Purpose
The algorithm of the local planner is updated as to accomodate changes in the map, which render previously planned paths in collision.
Changes
Previously, as soon as a plan through some waypoints is found, we set the next waypoint to the end of this list, regardless of the current position of the robot. If the plan at a later point is in collision, the replanning simply tracks to this next waypoint. However, if the waypoint is not trivially reachable, the local planner fails.
The new algorithm replans the path periodically to adjust to the updated map. We only iterate to the next waypoint as soon as the latest goal is reached. If a previously valid plan is in colliison, we use the trajectory as an initial guess for the replanning. This results in a more robust behavior.
Dependencies
Depends on this PR, which enables the access to the interpolated voxel weight, used to determine whether a position in the map is occupied or simply unknown.
Purpose The algorithm of the local planner is updated as to accomodate changes in the map, which render previously planned paths in collision.
Changes Previously, as soon as a plan through some waypoints is found, we set the next waypoint to the end of this list, regardless of the current position of the robot. If the plan at a later point is in collision, the replanning simply tracks to this next waypoint. However, if the waypoint is not trivially reachable, the local planner fails. The new algorithm replans the path periodically to adjust to the updated map. We only iterate to the next waypoint as soon as the latest goal is reached. If a previously valid plan is in colliison, we use the trajectory as an initial guess for the replanning. This results in a more robust behavior.
Dependencies Depends on this PR, which enables the access to the interpolated voxel weight, used to determine whether a position in the map is occupied or simply unknown.