cmeel-octomap

from octomap import OcTree, Point3D # Create an OcTree with a resolution of 0.1 meters tree = OcTree(0.1) # Insert some occupied points tree.insert_point(Point3D(0.0, 0.0, 0.0), occupied=True) tree.insert_point(Point3D(0.1, 0.0, 0.0), occupied=True) tree.insert_point(Point3D(0.5, 0.5, 0.5), occupied=True) # Insert a free point (clears any occupied status at this node) tree.insert_point(Point3D(0.0, 0.1, 0.0), occupied=False) # Query occupancy of points is_occupied_000 = tree.is_node_occupied(Point3D(0.0, 0.0, 0.0)) is_occupied_055 = tree.is_node_occupied(Point3D(0.5, 0.5, 0.5)) is_occupied_001 = tree.is_node_occupied(Point3D(0.0, 0.0, 1.0)) print(f"Point (0,0,0) is occupied: {is_occupied_000}") print(f"Point (0.5,0.5,0.5) is occupied: {is_occupied_055}") print(f"Point (0,0,1) is occupied: {is_occupied_001} (outside inserted points)") # Get the total number of occupied nodes print(f"Total occupied nodes: {tree.num_occupied_nodes}") # You can also save and load the map (uncomment to run) # tree.write_binary("my_octomap.bt") # loaded_tree = OcTree("my_octomap.bt") # print(f"Loaded tree has {loaded_tree.num_occupied_nodes} occupied nodes.")