feat(PlannerServer): BaseGlobalPlanner plugin accepts viapoints as part of API

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp

@@ -16,6 +16,7 @@
 #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
 
 #include <string>
+#include <vector>
 
 #include "nav2_msgs/action/compute_path_to_pose.hpp"
 #include "nav_msgs/msg/path.hpp"
@@ -94,6 +95,9 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
           "start",
           "Used as the planner start pose instead of the current robot pose, if use_start is"
           " not false (i.e. not provided or set to true)"),
+        BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+          "viapoints",
+          "A list of intermediate viapoints (excluding goal) to consider for planning"),
         BT::InputPort<bool>(
           "use_start", "For using or not using (i.e. ignoring) the provided start pose"),
         BT::InputPort<std::string>(

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -96,6 +96,7 @@
     <Action ID="ComputePathToPose">
       <input_port name="goal">Destination to plan to</input_port>
       <input_port name="start">Use as the planner start pose instead of the current robot pose, if use_start is not false (i.e. not provided or set to true)</input_port>
+      <input_port name="viapoints">A vector of intermediate viapoints (excluding goal) to consider for planning</input_port>
       <input_port name="use_start">For using or not using (i.e. ignoring) the provided start pose</input_port>
       <input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
       <input_port name="server_name">Server name</input_port>

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -33,6 +33,11 @@ void ComputePathToPoseAction::on_tick()
   getInput("goal", goal_.goal);
   getInput("planner_id", goal_.planner_id);
 
+  // use waypoints if available
+  if (!getInput("viapoints", goal_.viapoints)) {
+    goal_.viapoints = std::vector<geometry_msgs::msg::PoseStamped>();
+  }
+
   // if "use_start" is provided try to enforce it (true or false), but we cannot enforce true if
   // start is not provided
   goal_.use_start = false;

nav2_core/include/nav2_core/global_planner.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "tf2_ros/buffer.hpp"
 #include "nav_msgs/msg/path.hpp"
@@ -67,15 +68,17 @@ class GlobalPlanner
   virtual void deactivate() = 0;
 
   /**
-   * @brief Method create the plan from a starting and ending goal.
+   * @brief Method to create the plan from a starting pose, a goal pose, and intermediate viapoints.
    * @param start The starting pose of the robot
    * @param goal  The goal pose of the robot
+   * @param viapoints The intermediate viapoints for the robot
    * @param cancel_checker Function to check if the action has been canceled
    * @return      The sequence of poses to get from start to goal, if any
    */
   virtual nav_msgs::msg::Path createPlan(
     const geometry_msgs::msg::PoseStamped & start,
     const geometry_msgs::msg::PoseStamped & goal,
+    const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
     std::function<bool()> cancel_checker) = 0;
 };
 

nav2_msgs/action/ComputePathToPose.action

@@ -1,6 +1,7 @@
 #goal definition
 geometry_msgs/PoseStamped goal
 geometry_msgs/PoseStamped start
+geometry_msgs/PoseStamped[] viapoints
 string planner_id
 bool use_start # If false, use current robot pose as path start, if true, use start above instead
 ---

nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

@@ -88,6 +88,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner
   nav_msgs::msg::Path createPlan(
     const geometry_msgs::msg::PoseStamped & start,
     const geometry_msgs::msg::PoseStamped & goal,
+    const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
     std::function<bool()> cancel_checker) override;
 
 protected:

nav2_navfn_planner/src/navfn_planner.cpp

@@ -120,11 +120,18 @@ NavfnPlanner::cleanup()
 nav_msgs::msg::Path NavfnPlanner::createPlan(
   const geometry_msgs::msg::PoseStamped & start,
   const geometry_msgs::msg::PoseStamped & goal,
+  const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
   std::function<bool()> cancel_checker)
 {
 #ifdef BENCHMARK_TESTING
   steady_clock::time_point a = steady_clock::now();
 #endif
+
+  if (!viapoints.empty()) {
+    RCLCPP_WARN(logger_, "Received %zu viapoints, but this planner ignores them",
+      viapoints.size());
+  }
+
   std::lock_guard<std::mutex> lock_reinit(param_handler_->getMutex());
   unsigned int mx_start, my_start, mx_goal, my_goal;
   if (!costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) {

nav2_planner/include/nav2_planner/planner_server.hpp

@@ -76,6 +76,7 @@ class PlannerServer : public nav2::LifecycleNode
   nav_msgs::msg::Path getPlan(
     const geometry_msgs::msg::PoseStamped & start,
     const geometry_msgs::msg::PoseStamped & goal,
+    const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
     const std::string & planner_id,
     std::function<bool()> cancel_checker);
 

nav2_planner/src/planner_server.cpp

@@ -394,8 +394,9 @@ void PlannerServer::computePlanThroughPoses()
 
       // Get plan from start -> goal
       nav_msgs::msg::Path curr_path;
+      std::vector<geometry_msgs::msg::PoseStamped> viapoints;
       try {
-        curr_path = getPlan(curr_start, curr_goal, goal->planner_id, cancel_checker);
+        curr_path = getPlan(curr_start, curr_goal, viapoints, goal->planner_id, cancel_checker);
       } catch (nav2_core::PlannerException & ex) {
         if (i == 0 || !params_->partial_plan_allowed) {
           throw;
@@ -546,7 +547,7 @@ PlannerServer::computePlan()
         return action_server_pose_->is_cancel_requested();
       };
 
-    result->path = getPlan(start, goal_pose, goal->planner_id, cancel_checker);
+    result->path = getPlan(start, goal_pose, goal->viapoints, goal->planner_id, cancel_checker);
 
     if (!validatePath<ActionThroughPoses>(goal_pose, result->path, goal->planner_id)) {
       throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path");
@@ -615,6 +616,7 @@ nav_msgs::msg::Path
 PlannerServer::getPlan(
   const geometry_msgs::msg::PoseStamped & start,
   const geometry_msgs::msg::PoseStamped & goal,
+  const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
   const std::string & planner_id,
   std::function<bool()> cancel_checker)
 {
@@ -624,14 +626,15 @@ PlannerServer::getPlan(
     goal.pose.position.x, goal.pose.position.y);
 
   if (planners_.find(planner_id) != planners_.end()) {
-    return planners_[planner_id]->createPlan(start, goal, cancel_checker);
+    return planners_[planner_id]->createPlan(start, goal, viapoints, cancel_checker);
   } else {
     if (planners_.size() == 1 && planner_id.empty()) {
       RCLCPP_WARN_ONCE(
         get_logger(), "No planners specified in action call. "
         "Server will use only plugin %s in server."
         " This warning will appear once.", planner_ids_concat_.c_str());
-      return planners_[planners_.begin()->first]->createPlan(start, goal, cancel_checker);
+      return planners_[planners_.begin()->first]->createPlan(start, goal, viapoints,
+        cancel_checker);
     } else {
       RCLCPP_ERROR(
         get_logger(), "planner %s is not a valid planner. "

nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp

@@ -88,6 +88,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner
   nav_msgs::msg::Path createPlan(
     const geometry_msgs::msg::PoseStamped & start,
     const geometry_msgs::msg::PoseStamped & goal,
+    const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
     std::function<bool()> cancel_checker) override;
 
 protected:

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -87,6 +87,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   nav_msgs::msg::Path createPlan(
     const geometry_msgs::msg::PoseStamped & start,
     const geometry_msgs::msg::PoseStamped & goal,
+    const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
     std::function<bool()> cancel_checker)  override;
 
 protected:

nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

@@ -86,6 +86,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
   nav_msgs::msg::Path createPlan(
     const geometry_msgs::msg::PoseStamped & start,
     const geometry_msgs::msg::PoseStamped & goal,
+    const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
     std::function<bool()> cancel_checker) override;
 
 protected:

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -194,8 +194,14 @@ void SmacPlanner2D::cleanup()
 nav_msgs::msg::Path SmacPlanner2D::createPlan(
   const geometry_msgs::msg::PoseStamped & start,
   const geometry_msgs::msg::PoseStamped & goal,
+  const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
   std::function<bool()> cancel_checker)
 {
+  if (!viapoints.empty()) {
+    RCLCPP_WARN(_logger, "Received %zu viapoints, but this planner ignores them",
+      viapoints.size());
+  }
+
   std::lock_guard<std::mutex> lock_reinit(_mutex);
   steady_clock::time_point a = steady_clock::now();
 

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -339,8 +339,14 @@ void SmacPlannerHybrid::cleanup()
 nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   const geometry_msgs::msg::PoseStamped & start,
   const geometry_msgs::msg::PoseStamped & goal,
+  const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
   std::function<bool()> cancel_checker)
 {
+  if (!viapoints.empty()) {
+    RCLCPP_WARN(_logger, "Received %zu viapoints, but this planner ignores them",
+      viapoints.size());
+  }
+
   std::lock_guard<std::mutex> lock_reinit(_mutex);
   steady_clock::time_point a = steady_clock::now();
 

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -300,8 +300,14 @@ void SmacPlannerLattice::cleanup()
 nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   const geometry_msgs::msg::PoseStamped & start,
   const geometry_msgs::msg::PoseStamped & goal,
+  const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
   std::function<bool()> cancel_checker)
 {
+  if (!viapoints.empty()) {
+    RCLCPP_WARN(_logger, "Received %zu viapoints, but this planner ignores them",
+      viapoints.size());
+  }
+
   std::lock_guard<std::mutex> lock_reinit(_mutex);
   steady_clock::time_point a = steady_clock::now();
 

nav2_smac_planner/test/test_smac_2d.cpp

@@ -56,27 +56,32 @@ TEST(SmacTest, test_smac_2d) {
       return false;
     };
 
-  geometry_msgs::msg::PoseStamped start, goal;
+  geometry_msgs::msg::PoseStamped start, goal, viapoint;
   start.pose.position.x = 0.0;
   start.pose.position.y = 0.0;
   start.pose.orientation.w = 1.0;
   // goal = start;
   goal.pose.position.x = 7.0;
   goal.pose.position.y = 0.0;
   goal.pose.orientation.w = 1.0;
+  // viapoint = start;
+  goal.pose.position.x = 3.5;
+  goal.pose.position.y = 0.0;
+  goal.pose.orientation.w = 1.0;
+  std::vector<geometry_msgs::msg::PoseStamped> viapoints{viapoint};
   auto planner_2d = std::make_unique<nav2_smac_planner::SmacPlanner2D>();
   planner_2d->configure(node2D, "test", nullptr, costmap_ros);
   planner_2d->activate();
   try {
-    planner_2d->createPlan(start, goal, dummy_cancel_checker);
+    planner_2d->createPlan(start, goal, viapoints, dummy_cancel_checker);
   } catch (...) {
   }
 
   // corner case where the start and goal are on the same cell
   goal.pose.position.x = 0.01;
   goal.pose.position.y = 0.01;
 
-  nav_msgs::msg::Path plan = planner_2d->createPlan(start, goal, dummy_cancel_checker);
+  nav_msgs::msg::Path plan = planner_2d->createPlan(start, goal, viapoints, dummy_cancel_checker);
   EXPECT_EQ(plan.poses.size(), 1);  // single point path
 
   planner_2d->deactivate();

nav2_smac_planner/test/test_smac_hybrid.cpp

@@ -109,13 +109,18 @@ TEST(SmacTest, test_smac_se2)
       return false;
     };
 
-  geometry_msgs::msg::PoseStamped start, goal;
+  std::vector<geometry_msgs::msg::PoseStamped> no_viapoints{};
+  geometry_msgs::msg::PoseStamped start, goal, viapoint;
   start.pose.position.x = 0.0;
   start.pose.position.y = 0.0;
   start.pose.orientation.w = 1.0;
   goal.pose.position.x = 1.0;
   goal.pose.position.y = 1.0;
   goal.pose.orientation.w = 1.0;
+  viapoint.pose.position.x = 0.5;
+  viapoint.pose.position.y = 0.5;
+  viapoint.pose.orientation.w = 1.0;
+  std::vector<geometry_msgs::msg::PoseStamped> viapoints{viapoint};
   auto planner = std::make_unique<HybridWrap>();
 
   // invalid goal heading mode
@@ -160,7 +165,7 @@ TEST(SmacTest, test_smac_se2)
   planner->activate();
 
   try {
-    planner->createPlan(start, goal, dummy_cancel_checker);
+    planner->createPlan(start, goal, viapoints, dummy_cancel_checker);
   } catch (...) {
   }
 
@@ -186,7 +191,8 @@ TEST(SmacTest, test_smac_se2)
   goal.pose.position.x = 0.01;
   goal.pose.position.y = 0.01;
 
-  nav_msgs::msg::Path plan = planner->createPlan(start, goal, dummy_cancel_checker);
+  nav_msgs::msg::Path plan = planner->createPlan(
+    start, goal, no_viapoints, dummy_cancel_checker);
   EXPECT_EQ(plan.poses.size(), 1);  // single point path
 
   auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
@@ -200,7 +206,8 @@ TEST(SmacTest, test_smac_se2)
   executor.spin_until_future_complete(results);
   goal.pose.position.x = 4.0;
   goal.pose.position.y = 4.0;
-  EXPECT_THROW(planner->createPlan(start, goal, dummy_cancel_checker), std::runtime_error);
+  EXPECT_THROW(planner->createPlan(
+    start, goal, no_viapoints, dummy_cancel_checker), std::runtime_error);
 
   planner->deactivate();
   planner->cleanup();

nav2_smac_planner/test/test_smac_lattice.cpp

@@ -121,13 +121,18 @@ TEST(SmacTest, test_smac_lattice)
       return false;
     };
 
-  geometry_msgs::msg::PoseStamped start, goal;
+  std::vector<geometry_msgs::msg::PoseStamped> no_viapoints{};
+  geometry_msgs::msg::PoseStamped start, goal, viapoint;
   start.pose.position.x = 0.0;
   start.pose.position.y = 0.0;
   start.pose.orientation.w = 1.0;
   goal.pose.position.x = 1.0;
   goal.pose.position.y = 1.0;
   goal.pose.orientation.w = 1.0;
+  viapoint.pose.position.x = 0.5;
+  viapoint.pose.position.y = 0.5;
+  viapoint.pose.orientation.w = 1.0;
+  std::vector<geometry_msgs::msg::PoseStamped> viapoints{viapoint};
   auto planner = std::make_unique<LatticeWrap>();
   try {
     // invalid goal heading mode
@@ -165,7 +170,7 @@ TEST(SmacTest, test_smac_lattice)
   planner->activate();
 
   try {
-    planner->createPlan(start, goal, dummy_cancel_checker);
+    planner->createPlan(start, goal, viapoints, dummy_cancel_checker);
   } catch (...) {
   }
 
@@ -192,7 +197,7 @@ TEST(SmacTest, test_smac_lattice)
   goal.pose.position.x = 0.01;
   goal.pose.position.y = 0.01;
 
-  nav_msgs::msg::Path plan = planner->createPlan(start, goal, dummy_cancel_checker);
+  nav_msgs::msg::Path plan = planner->createPlan(start, goal, no_viapoints, dummy_cancel_checker);
   EXPECT_EQ(plan.poses.size(), 1);  // single point path
 
   auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
@@ -206,7 +211,8 @@ TEST(SmacTest, test_smac_lattice)
   executor.spin_until_future_complete(results);
   goal.pose.position.x = 4.0;
   goal.pose.position.y = 4.0;
-  EXPECT_THROW(planner->createPlan(start, goal, dummy_cancel_checker), std::runtime_error);
+  EXPECT_THROW(planner->createPlan(
+    start, goal, no_viapoints, dummy_cancel_checker), std::runtime_error);
 
   planner->deactivate();
   planner->cleanup();