Python Bindings for OctoMap

import pyoctomap as pom
import numpy as np

# Standard occupancy mapping
occ = pom.OcTree(0.1)
occ.updateNode([1.0, 2.0, 3.0], True)

# Color occupancy mapping
color = pom.ColorOcTree(0.1)
coord = [1.0, 1.0, 1.0]
color.updateNode(coord, True)
color.setNodeColor(coord, 255, 0, 0)

# Batch insertion with colors
points = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], dtype=np.float64)
colors = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]], dtype=np.float64)  # RGB in [0, 1]
color.insertPointCloud(points, colors=colors)

# Counting tree - integer hit statistics
count = pom.CountingOcTree(0.1)
count_node = count.updateNode([0.0, 0.0, 0.0])
print("Hits:", count_node.getCount())

# Time-stamped occupancy - temporal maps
stamped = pom.OcTreeStamped(0.1)
stamped.updateNode([2.0, 0.0, 1.0], True)
print("Last update time:", stamped.getLastUpdateTime())

# Batch insertion with timestamps
points = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], dtype=np.float64)
timestamp = 12345  # unsigned int
stamped.insertPointCloud(points, timestamps=timestamp)

import pyoctomap as pom
import numpy as np

tree = pom.OcTree(0.1)
points = np.random.rand(1000, 3) * 10.0
origin = np.array([0.0, 0.0, 1.5])

# Insert point cloud (synchronous update inner nodes)
tree.insertPointCloud(
    points,
    origin,
    max_range=50.0,
    lazy_eval=False
)

# Insert point cloud with lazy evaluation (efficient for multiple scans)
tree.insertPointCloud(points, origin, lazy_eval=True)
tree.updateInnerOccupancy()

# Color tree: batch insertion with colors
color_tree = pom.ColorOcTree(0.1)
colors = np.random.rand(1000, 3)  # RGB in [0, 1] range
color_tree.insertPointCloud(points, colors=colors)

# Stamped tree: batch insertion with timestamps
stamped_tree = pom.OcTreeStamped(0.1)
timestamp = 12345  # unsigned int
stamped_tree.insertPointCloud(points, timestamps=timestamp)

# Create an octree for path planning
tree = pom.OcTree(0.1)  # 10cm resolution

# Add some obstacles to the map
obstacles = [
    [1.0, 1.0, 0.5],  # Wall at (1,1)
    [1.5, 1.5, 0.5],  # Another obstacle
    [2.0, 1.0, 0.5],  # Wall at (2,1)
]

for obstacle in obstacles:
    tree.updateNode(obstacle, True)

def is_path_clear(start, end, tree):
    """Efficient ray casting for path planning using OctoMap's built-in castRay"""
    start = np.array(start, dtype=np.float64)
    end = np.array(end, dtype=np.float64)
    
    # Calculate direction vector
    direction = end - start
    ray_length = np.linalg.norm(direction)
    
    if ray_length == 0:
        return True, None
    
    # Normalize direction
    direction = direction / ray_length
    
    # Use OctoMap's efficient castRay method
    end_point = np.zeros(3, dtype=np.float64)
    hit = tree.castRay(start, direction, end_point, 
                        ignoreUnknownCells=True, 
                        maxRange=ray_length)
    
    if hit:
        # Ray hit an obstacle - path is blocked
        return False, end_point
    else:
        # No obstacle found - path is clear
        return True, None

# Check if path is clear
start = [0.5, 2.0, 0.5]
end = [2.0, 2.0, 0.5]
clear, obstacle = is_path_clear(start, end, tree)
if clear:
    print("✅ Path is clear!")
else:
    print(f"❌ Path blocked at: {obstacle}")

# Advanced path planning with multiple waypoints
def plan_path(waypoints, tree):
    """Plan a path through multiple waypoints using ray casting"""
    path_clear = True
    obstacles = []
    
    for i in range(len(waypoints) - 1):
        start = waypoints[i]
        end = waypoints[i + 1]
        clear, obstacle = is_path_clear(start, end, tree)
        
        if not clear:
            path_clear = False
            obstacles.append((i, i+1, obstacle))
    
    return path_clear, obstacles

# Example: Plan path through multiple waypoints
waypoints = [
    [0.0, 0.0, 0.5],
    [1.0, 1.0, 0.5], 
    [2.0, 2.0, 0.5],
    [3.0, 3.0, 0.5]
]

path_clear, obstacles = plan_path(waypoints, tree)
if path_clear:
    print("✅ Complete path is clear!")
else:
    print(f"❌ Path blocked at segments: {obstacles}")