Using the map for collision checking/planning

[Aldhra]

Hello,

I've been playing around with some your code to construct some maps using a Robosense RS-Bpearl and I like the results, thanks for the work.

I wanted to see if i could check for collision based on our robot pose and radius. Reading the documentation and the examples : they show how to check a single cell for occupancy values but how would you recommend to iterate through the map efficiently, and to use multi resolution to optimize the search from coarse to fine ?

[victorreijgwart]

Hi,
You can speed up map queries using the query accelerator. It speeds up the query by caching the ancestors (hash-block + nodes from the Octree root down to the leaf) of the previous query. For the next query, it then only needs to traverse the part of the Octree that differs from the previous query.
The API currently provides two types of iterators that could be useful for path planning, a grid iterator (which you can for example use to quickly iterate over all the cells in an AABB) and a ray iterator. These both operate at a fixed resolution, but the resolution does not have to be the maximum. You can extend this example to compute the start and end Octree indices at the resolution you care about, initialize the grid/ray iterator with the min_index.position, max_index.position, and then iterate over the map at the resolution you selected.
In our own current research, we are working on multi-resolution motion planning algorithms. For this we mostly write custom recursive functions, since these let you make full use of the map's hierarchical structure. These are very similar to the algorithms we use to integrate measurements into the map, for example like this for recursion with a stack or like this for regular recursion. In general, I'd recommend starting with regular recursion, as it's more readable and in our benchmarking, the performance was very similar between both.
We intend to extend the API with multi-resolution collision checkers, ESDF generators, planners, and other helper functions. But probably not in the next few months, as we have a lot of deadlines coming up. In the meantime, don't hesitate if you have more questions.

[victorreijgwart]

@Aldhra, I'll close this issue, assuming your question is answered. Don't hesitate to re-open it if you have follow-up questions.