Overview
The project implements publishers, subscribers, services, and an external rosbridge-style gateway without ROS. The planner converts world coordinates into cells and publishes a cell-center path on /path.
The interesting constraint: /heap_sort is a public service and a required dependency of planning.
Algorithm, in a loop
- Validate the map; occupancy ≥ 50 is blocked.
- Convert start and goal using origin and resolution.
- Pop the lowest frontier entry, expand four neighbors, and update
f = g + h. - Backtrack parents and return cell centers.
Code map
world pose → grid cell → min-heap frontier → four neighbors · origin + resolution · occupancy check · heap_sort → /plan_path → /path
Key files include p1/starter/cpp/src/astar.cpp, the heap implementations, and middleware.cpp.