Costmap Path Planning

Context: Last season, we implemented obstacle avoidance during the water bottle search (project outlined here). Due to us encountering more obstacles than anticipated, we'd like to expand our usage of obstacle avoidance beyond just the water bottle search.

Problem: We currently only use the costmap to avoid obstacles when in WaterBottleSearchState, but we anticipate there possibly being obstacles during other parts of the auton mission.

Solution: We will integrate the costmap functionality from WaterBottleSearchState into the general SearchState, so that it can be used when searching for any target. Additionally, we may experiment with adding the costmap to WaypointState, so that we can avoid obstacles when traversing larger distances between waypoints. We will also implement an interface with teleop so that the costmap functionality can be toggled on or off—this can be used a safety switch in case the obstacle avoidance isn't functioning as intended, and it can be used to disable obstacle avoidance when the terrain clearly does not require it.

Here's a (simplified) overview of the navigation state machine as it is now:

For more details on the specifics of how the current costmap implementation works, be sure to check out this page.