Overview
I wrote every navigation node between the lidar scan and velocity command.
Each scan becomes a local costmap, then a persistent world map. A* plans through that map, and a Pure Pursuit controller turns the path into velocity commands. I implemented all four nodes and debugged the places where they meet: coordinate transforms, occupancy values, map updates, replanning, and path tracking.
How it works
From lidar scan to velocity command.
-
01
Local costmap
LaserScan→/costmapI used Bresenham ray tracing to mark free space along each lidar beam, then restored and inflated obstacle cells around the robot footprint. Unseen cells remained unknown rather than being treated as clear.
-
02
Map memory
/costmap+ odometry →/mapI transformed each local grid into world coordinates using odometry and merged it into a persistent map. Fresh observations could clear stale costs without overwriting unseen areas.
-
03
A* planner
/map+ goal →/pathI implemented four-connected A* with traversal costs for uncertain and inflated cells. The planner replanned after map updates and rejected invalid or occupied goals.
-
04
Path controller
/path+ odometry →/cmd_velI selected forward lookahead points, calculated Pure Pursuit curvature, and published linear and angular velocity commands. The controller stopped for invalid paths and at the goal.
Debugging
Debugging across four ROS 2 nodes.
When the map, path, or controller behaved unexpectedly, I traced the data through each topic instead of treating the symptom in isolation.
Radial map artifacts
Fixed the distinction between unknown and free space.
I kept unknown, free, and occupied cells separate. Bresenham ray tracing marked free space along each beam, then obstacle endpoints were restored and inflated around the robot footprint. New scans could clear stale costs.
Inflation deadlock
Recovered safely when inflation covered the start cell.
I added a bounded search from the robot's current cell toward lower-cost space. If it could not reach a safe start without crossing a lethal cell, the planner stopped instead of forcing a route.
Controller reversal
Kept lookahead points ahead of the robot.
The controller first found the path point nearest the robot, then searched forward from there. That kept passed waypoints from pulling the robot backward or causing a stall.
Unstable turns
Handled large heading errors before moving forward.
The robot turned in place before forward tracking resumed. I also tapered speed through curves and clamped angular commands to keep motion predictable.
Result
A complete navigation run.
The video shows the robot building its map, updating the route around inflated obstacles, and following that path to the selected goal.
I used Foxglove to watch the lidar, stored map, active path, and robot response together while testing the stack in Gazebo.