This week I worked on getting the full autonomous navigation stack running on the robot. Most of the time was spent debugging the ROS2 TF tree on the Pi 5 — Cartographer needs a specific chain of coordinate transforms (map -> odom -> base_link -> laser_frame) and getting all of those publishing correctly without conflicts took a while. The main issues were the robot_state_publisher broadcasting stale timestamps for the laser frame, and Cartographer conflicting with the OTOS odometry node when the wrong frame was set as the published frame.
Once the TF tree was stable I integrated the SparkFun OTOS optical flow sensor as the odometry source for Cartographer. Got it publishing over I2C, converting units correctly, and matching the QoS settings Cartographer expects. Did some test mapping sessions and got a clean room map — sensor height matters a lot, table height produced much better maps than floor level because the LiDAR could see clean wall-to-wall without furniture legs getting in the way.
After mapping I set up the full Nav2 navigation stack. Decided to use Cartographer in localization mode instead of AMCL since the Pi 5 has enough CPU headroom and it gives better accuracy. Wrote all the config and launch files for that.
Also wrote the motor controller node for the VNH5019 drivers using hardware PWM at 20kHz so the motors don’t whine, and the full human protocol system, a detection bridge node that reads JSON from the Kria over USB serial, a protocol manager that implements all five behavioral responses (normal, people nearby, fallen person, running person, crowd), and a speed limiter that sits between Nav2 and the motors to enforce speed limits per protocol. LED and buzzer control through GPIO is in there too.
Still need to physically mount the sensors on the chassis, wire up the motor drivers, and do a full end-to-end test with everything running together.