Feature request

Pass intermediate viapoints such as a reference route to the planner plugin to facilitate planning optimization

Feature description

The "ComputePath..." action will be able to consume a set of viapoints that can be accessed directly within a custom planner plugin. This will:

• Allow planning algorithms to inherently leverage the global context of a desired route during computation by implementing optimizations at the exploration stage for example.
• Eliminate the overhead of multiple calls planner calls and corresponding initialization
• Eliminate the overhead of possibly complex stitching algorithms where needed
• Allow introducing tolerance for viapoint tracking

Implementation considerations

Reference branch: https://github.com/sbadamikar-research/nav2/tree/feature/plugin_viapoints

I intend to extend the BaseGlobalPlanner class to define an additional interface by overloading the createPlan function as follows:

virtual nav_msgs::msg::Path createPlan(
    const geometry_msgs::msg::PoseStamped & start,
    const geometry_msgs::msg::PoseStamped & goal,
    std::function<bool()> cancel_checker,
    const nav_msgs::Goals & viapoints ) { return createPlan(start, goal, cancel_checker); } 

This is then followed by updating ComputePlanToPose::Goal with an additional field for nav_msgs/Goals viapoints as well as the implementations of getPlan() within the PlannerServer to accept the viapoints and use them for the createPlan() call.

Since all plugins are stored in the planner map as GlobalPlanner::Ptr, existing plugins are backward compatible since the vtable entry for createPlan(..., viapoints) calls BaseGlobalPlanner::createPlan(..., viapoints) that in turn calls createPlan(...) for which, the vtable entry is the override in the derived implementation.

Any plugin can now implement the overloaded definition if needed and continue to use the original definition if not.

ComputePlanThroughPoses continues to execute getPlan() with an empty set of viapoints to maintain existing stitching functionality.

ComputePlanToPose consumes the viapoints and passes them to getPlan() for the eventual call to createPlan().

Since this change is not ABI-compatible, I presume it would be to Rolling?