Terrain-aware navigation & vehicle safety stack
I wrote a Nav2 costmap layer that scores terrain from 3D LiDAR by slope, roughness and step height. Alongside it I built a vehicle safety arbiter and a simulated CAN drive interface that supports fault injection.
1Problem
Standard obstacle layers see a ditch and a gentle slope as the same free space. Off-road, the robot kept getting stuck and running recovery behaviors. The vehicle side also needed its own health checks.
2What I built
- 01
Terrain costmap plugin
A C++ Nav2 layer that turns 3D LiDAR into slope, roughness, height-step and obstacle costs for the planner.
- 02
Safety arbiter
A Python node that switches between autonomy and teleoperation and watches e-stop, localization and odometry health, and timeouts. It stops the vehicle in a controlled way and logs events.
- 03
Simulated CAN drive bus
CAN/DBC messages with CRC-8, rolling counters, heartbeats and ECU watchdogs. Faults can be injected, and the drive has to be re-armed explicitly afterward.
- 04
Testing
gtest and pytest suites, plus repeatable route trials comparing the default and terrain-aware setups.
3Result
In the documented evaluation, the terrain-aware configuration cut median recoveries from 20 to 3 and route time from 318 s to 113 s.
| Metric | Before | After | Unit |
|---|---|---|---|
| Median recoveries | 20 | 3 | |
| Route time | 318 | 113 | s |