Initial draft of Controller Path Following Deviation Range API

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp

@@ -17,6 +17,7 @@
 
 #include <string>
 #include <memory>
+#include <vector>
 
 #include "nav2_msgs/action/follow_path.hpp"
 #include "nav2_behavior_tree/bt_action_node.hpp"
@@ -89,13 +90,16 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
   {
     // Register JSON definitions for the types used in the ports
     BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+    BT::RegisterJsonDefinition<std::vector<geometry_msgs::msg::PoseStamped>>();
 
     return providedBasicPorts(
       {
         BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
         BT::InputPort<std::string>("controller_id", ""),
         BT::InputPort<std::string>("goal_checker_id", ""),
         BT::InputPort<std::string>("progress_checker_id", ""),
+        BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>("key_poses",
+          "Key poses along the path"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The follow path error code"),
         BT::OutputPort<std::string>(

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -34,6 +34,7 @@
   getInput("controller_id", goal_.controller_id);
   getInput("goal_checker_id", goal_.goal_checker_id);
   getInput("progress_checker_id", goal_.progress_checker_id);
+  getInput("key_poses", goal_.key_poses);
 }
 
 BT::NodeStatus FollowPathAction::on_success()
@@ -101,6 +102,14 @@
     goal_.progress_checker_id = new_progress_checker_id;
     goal_updated_ = true;
   }
+
+  std::vector<geometry_msgs::msg::PoseStamped> new_key_poses;
+  getInput("key_poses", new_key_poses);
+
+  if (goal_.key_poses != new_key_poses) {
+    goal_.key_poses = new_key_poses;
+    goal_updated_ = true;
+  }
 }
 
 }  // namespace nav2_behavior_tree

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -156,8 +156,11 @@ class ControllerServer : public nav2::LifecycleNode
   /**
    * @brief Assigns path to controller
    * @param path Path received from action server
+   * @param key_poses Key poses along the path that are considered points of interest
    */
-  void setPlannerPath(const nav_msgs::msg::Path & path);
+  void setPlannerPath(
+    const nav_msgs::msg::Path & path,
+    const std::vector<geometry_msgs::msg::PoseStamped> & key_poses);
   /**
    * @brief Calculates velocity and publishes to "cmd_vel" topic
    */

nav2_controller/src/controller_server.cpp

@@ -457,7 +457,7 @@ void ControllerServer::computeControl()
       throw nav2_core::ControllerException("Failed to find progress checker name: " + pc_name);
     }
 
-    setPlannerPath(goal->path);
+    setPlannerPath(goal->path, goal->key_poses);
     progress_checkers_[current_progress_checker_]->reset();
 
     last_valid_cmd_time_ = now();
@@ -591,15 +591,17 @@ void ControllerServer::computeControl()
   action_server_->succeeded_current();
 }
 
-void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
+void ControllerServer::setPlannerPath(
+  const nav_msgs::msg::Path & path,
+  const std::vector<geometry_msgs::msg::PoseStamped> & key_poses)
 {
   RCLCPP_DEBUG(
     get_logger(),
     "Providing path to the controller %s", current_controller_.c_str());
   if (path.poses.empty()) {
     throw nav2_core::InvalidPath("Path is empty.");
   }
-  controllers_[current_controller_]->setPlan(path);
+  controllers_[current_controller_]->setPlan(path, key_poses);
 
   end_pose_ = path.poses.back();
   end_pose_.header.frame_id = path.header.frame_id;
@@ -743,7 +745,7 @@ void ControllerServer::updateGlobalPath()
       action_server_->terminate_current(result);
       return;
     }
-    setPlannerPath(goal->path);
+    setPlannerPath(goal->path, goal->key_poses);
   }
 }
 

nav2_core/include/nav2_core/controller.hpp

@@ -38,6 +38,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
@@ -94,8 +95,11 @@ class Controller
   /**
    * @brief local setPlan - Sets the global plan
    * @param path The global plan
+   * @param key_poses Key poses along the path that are considered points of interest
    */
-  virtual void setPlan(const nav_msgs::msg::Path & path) = 0;
+  virtual void setPlan(
+    const nav_msgs::msg::Path & path,
+    const std::vector<geometry_msgs::msg::PoseStamped> & key_poses) = 0;
 
   /**
    * @brief Controller computeVelocityCommands - calculates the best command given the current pose and velocity

nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp

@@ -91,8 +91,11 @@ class DWBLocalPlanner : public nav2_core::Controller
   /**
    * @brief nav2_core setPlan - Sets the global plan
    * @param path The global plan
+   * @param key_poses Key poses along the path that are considered points of interest
    */
-  void setPlan(const nav_msgs::msg::Path & path) override;
+  void setPlan(
+    const nav_msgs::msg::Path & path,
+    const std::vector<geometry_msgs::msg::PoseStamped> & key_poses) override;
 
   /**
    * @brief nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity

nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp

@@ -235,7 +235,9 @@ DWBLocalPlanner::loadCritics()
 }
 
 void
-DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path)
+DWBLocalPlanner::setPlan(
+  const nav_msgs::msg::Path & path,
+  const std::vector<geometry_msgs::msg::PoseStamped> & /*key_poses*/)
 {
   auto path2d = nav_2d_utils::pathToPath2D(path);
   for (TrajectoryCritic::Ptr & critic : critics_) {

nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp

@@ -94,8 +94,11 @@ class GracefulController : public nav2_core::Controller
   /**
    * @brief nav2_core setPlan - Sets the global plan.
    * @param path The global plan
+   * @param key_poses Key poses along the path that are considered points of interest
    */
-  void setPlan(const nav_msgs::msg::Path & path) override;
+  void setPlan(
+    const nav_msgs::msg::Path & path,
+    const std::vector<geometry_msgs::msg::PoseStamped> & key_poses) override;
 
   /**
    * @brief Limits the maximum linear speed of the robot.

nav2_graceful_controller/src/graceful_controller.cpp

@@ -252,7 +252,9 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
   throw nav2_core::NoValidControl("Collision detected in trajectory");
 }
 
-void GracefulController::setPlan(const nav_msgs::msg::Path & path)
+void GracefulController::setPlan(
+  const nav_msgs::msg::Path & path,
+  const std::vector<geometry_msgs::msg::PoseStamped> & /*key_poses*/)
 {
   path_handler_->setPlan(path);
   goal_reached_ = false;

nav2_graceful_controller/test/test_graceful_controller.cpp

@@ -225,7 +225,7 @@ TEST(GracefulControllerTest, configure) {
   plan.header.frame_id = "map";
   plan.poses.resize(3);
   plan.poses[0].header.frame_id = "map";
-  controller->setPlan(plan);
+  controller->setPlan(plan, {});
   EXPECT_EQ(controller->getPlan().poses.size(), 3u);
   EXPECT_EQ(controller->getPlan().poses[0].header.frame_id, "map");
 
@@ -536,7 +536,7 @@ TEST(GracefulControllerTest, emptyPlan) {
   // Set an empty global plan
   nav_msgs::msg::Path global_plan;
   global_plan.header.frame_id = "test_global_frame";
-  controller->setPlan(global_plan);
+  controller->setPlan(global_plan, {});
 
   EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::InvalidPath);
 }
@@ -591,7 +591,7 @@ TEST(GracefulControllerTest, poseOutsideCostmap) {
   global_plan.poses[0].header.frame_id = "test_global_frame";
   global_plan.poses[0].pose.position.x = 0.0;
   global_plan.poses[0].pose.position.y = 0.0;
-  controller->setPlan(global_plan);
+  controller->setPlan(global_plan, {});
 
   EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::ControllerException);
 }
@@ -655,7 +655,7 @@ TEST(GracefulControllerTest, noPruningPlan) {
   global_plan.poses[2].pose.position.x = 3.0;
   global_plan.poses[2].pose.position.y = 3.0;
   global_plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
-  controller->setPlan(global_plan);
+  controller->setPlan(global_plan, {});
 
   // Check results: the plan should not be pruned
   auto transformed_plan = controller->transformGlobalPlan(robot_pose);
@@ -732,7 +732,7 @@ TEST(GracefulControllerTest, pruningPlan) {
   global_plan.poses[5].pose.position.x = 500.0;
   global_plan.poses[5].pose.position.y = 500.0;
   global_plan.poses[5].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
-  controller->setPlan(global_plan);
+  controller->setPlan(global_plan, {});
 
   // Check results: the plan should be pruned
   auto transformed_plan = controller->transformGlobalPlan(robot_pose);
@@ -798,7 +798,7 @@ TEST(GracefulControllerTest, pruningPlanOutsideCostmap) {
   global_plan.poses[2].pose.position.x = 200.0;
   global_plan.poses[2].pose.position.y = 200.0;
   global_plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
-  controller->setPlan(global_plan);
+  controller->setPlan(global_plan, {});
 
   // Check results: the plan should be pruned
   auto transformed_plan = controller->transformGlobalPlan(robot_pose);
@@ -866,7 +866,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) {
   plan.poses[2].pose.position.x = 1.0;
   plan.poses[2].pose.position.y = 1.0;
   plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
-  controller->setPlan(plan);
+  controller->setPlan(plan, {});
 
   // Set velocity
   geometry_msgs::msg::Twist robot_velocity;
@@ -942,7 +942,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) {
   plan.poses[2].pose.position.x = 2.0;
   plan.poses[2].pose.position.y = 0.0;
   plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
-  controller->setPlan(plan);
+  controller->setPlan(plan, {});
 
   // Set velocity
   geometry_msgs::msg::Twist robot_velocity;
@@ -1023,7 +1023,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) {
   plan.poses[2].pose.position.x = -2.0;
   plan.poses[2].pose.position.y = 0.0;
   plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
-  controller->setPlan(plan);
+  controller->setPlan(plan, {});
 
   // Set velocity
   geometry_msgs::msg::Twist robot_velocity;
@@ -1108,7 +1108,7 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) {
   plan.poses[4].pose.position.x = 0.2;
   plan.poses[4].pose.position.y = 0.0;
   plan.poses[4].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
-  controller->setPlan(plan);
+  controller->setPlan(plan, {});
 
   // Set velocity
   geometry_msgs::msg::Twist robot_velocity;

nav2_mppi_controller/benchmark/controller_benchmark.cpp

@@ -106,7 +106,7 @@ void prepareAndRunBenchmark(
   auto velocity = getDummyTwist();
   auto path = getIncrementalDummyPath(node, path_settings);
 
-  controller->setPlan(path);
+  controller->setPlan(path, {});
 
   nav2_core::GoalChecker * dummy_goal_checker{nullptr};
 

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

@@ -17,6 +17,7 @@
 
 #include <string>
 #include <memory>
+#include <vector>
 
 #include "nav2_mppi_controller/tools/path_handler.hpp"
 #include "nav2_mppi_controller/optimizer.hpp"
@@ -90,8 +91,11 @@ class MPPIController : public nav2_core::Controller
   /**
     * @brief Set new reference path to track
     * @param path Path to track
+    * @param key_poses Key poses along the path that are considered points of interest
     */
-  void setPlan(const nav_msgs::msg::Path & path) override;
+  void setPlan(
+    const nav_msgs::msg::Path & path,
+    const std::vector<geometry_msgs::msg::PoseStamped> & key_poses) override;
 
   /**
     * @brief Set new speed limit from callback

nav2_mppi_controller/src/controller.cpp

@@ -141,7 +141,9 @@ void MPPIController::visualize(
   trajectory_visualizer_.visualize(std::move(transformed_plan));
 }
 
-void MPPIController::setPlan(const nav_msgs::msg::Path & path)
+void MPPIController::setPlan(
+  const nav_msgs::msg::Path & path,
+  const std::vector<geometry_msgs::msg::PoseStamped> & /*key_poses*/)
 {
   path_handler_.setPath(path);
 }

nav2_mppi_controller/test/controller_state_transition_test.cpp

@@ -56,7 +56,7 @@ TEST(ControllerStateTransitionTest, ControllerNotFail)
   path.header.frame_id = costmap_ros->getGlobalFrameID();
   pose.header.frame_id = costmap_ros->getGlobalFrameID();
 
-  controller->setPlan(path);
+  controller->setPlan(path, {});
 
   EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}));
 

nav2_msgs/action/FollowPath.action

@@ -3,6 +3,7 @@ nav_msgs/Path path
 string controller_id
 string goal_checker_id
 string progress_checker_id
+geometry_msgs/PoseStamped[] key_poses
 ---
 #result definition
 

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -102,8 +102,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   /**
    * @brief nav2_core setPlan - Sets the global plan
    * @param path The global plan
+   * @param key_poses Key poses along the path that are considered points of interest
    */
-  void setPlan(const nav_msgs::msg::Path & path) override;
+  void setPlan(
+    const nav_msgs::msg::Path & path,
+    const std::vector<geometry_msgs::msg::PoseStamped> & key_poses) override;
 
   /**
    * @brief Limits the maximum linear speed of the robot.

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -477,7 +477,9 @@ void RegulatedPurePursuitController::applyConstraints(
   linear_vel = sign * linear_vel;
 }
 
-void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path)
+void RegulatedPurePursuitController::setPlan(
+  const nav_msgs::msg::Path & path,
+  const std::vector<geometry_msgs::msg::PoseStamped> & /*key_poses*/)
 {
   has_reached_xy_tolerance_ = false;
   path_handler_->setPlan(path);