[controller_server] [FollowPath] Allow multiple progress checkers (ros-navigation#3555)

* allow multiple progress checker plugin

* merge conflict

* handle change of progress checker during a preempt

* reset pc on preempt

* fix default declaration

* Handle deprecated progress_checker_plugin

* improve readability

* Update nav2_controller/src/controller_server.cpp

---------

Co-authored-by: Guillaume Doisy <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

Author: 3 people

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

@@ -84,6 +84,7 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
         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::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The follow path error code"),
       });

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -114,7 +114,8 @@
       <input_port name="controller_id" default="FollowPath"/>
       <input_port name="path">Path to follow</input_port>
       <input_port name="goal_checker_id">Goal checker</input_port>
-      <input_port name="server_name">Server name</input_port>
+      <input_port name="progress_checker_id">Progress checker</input_port>
+      <input_port name="service_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="error_code_id">Follow Path error code</output_port>
     </Action>

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -33,6 +33,7 @@ void FollowPathAction::on_tick()
   getInput("path", goal_.path);
   getInput("controller_id", goal_.controller_id);
   getInput("goal_checker_id", goal_.goal_checker_id);
+  getInput("progress_checker_id", goal_.progress_checker_id);
 }
 
 BT::NodeStatus FollowPathAction::on_success()
@@ -83,6 +84,14 @@ void FollowPathAction::on_wait_for_result(
     goal_.goal_checker_id = new_goal_checker_id;
     goal_updated_ = true;
   }
+
+  std::string new_progress_checker_id;
+  getInput("progress_checker_id", new_progress_checker_id);
+
+  if (goal_.progress_checker_id != new_progress_checker_id) {
+    goal_.progress_checker_id = new_progress_checker_id;
+    goal_updated_ = true;
+  }
 }
 
 }  // namespace nav2_behavior_tree

nav2_bringup/params/nav2_multirobot_params_1.yaml

@@ -116,7 +116,7 @@ controller_server:
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
-    progress_checker_plugin: "progress_checker"
+    progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: ["goal_checker"]
     controller_plugins: ["FollowPath"]
 

nav2_bringup/params/nav2_multirobot_params_2.yaml

@@ -116,7 +116,7 @@ controller_server:
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
-    progress_checker_plugin: "progress_checker"
+    progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: ["goal_checker"]
     controller_plugins: ["FollowPath"]
 

nav2_bringup/params/nav2_params.yaml

@@ -117,7 +117,7 @@ controller_server:
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
     failure_tolerance: 0.3
-    progress_checker_plugin: "progress_checker"
+    progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
     controller_plugins: ["FollowPath"]
 

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -50,6 +50,7 @@ class ControllerServer : public nav2_util::LifecycleNode
 public:
   using ControllerMap = std::unordered_map<std::string, nav2_core::Controller::Ptr>;
   using GoalCheckerMap = std::unordered_map<std::string, nav2_core::GoalChecker::Ptr>;
+  using ProgressCheckerMap = std::unordered_map<std::string, nav2_core::ProgressChecker::Ptr>;
 
   /**
    * @brief Constructor for nav2_controller::ControllerServer
@@ -142,6 +143,15 @@ class ControllerServer : public nav2_util::LifecycleNode
    */
   bool findGoalCheckerId(const std::string & c_name, std::string & name);
 
+  /**
+   * @brief Find the valid progress checker ID name for the specified parameter
+   *
+   * @param c_name The progress checker name
+   * @param name Reference to the name to use for progress checking if any valid available
+   * @return bool Whether it found a valid progress checker to use
+   */
+  bool findProgressCheckerId(const std::string & c_name, std::string & name);
+
   /**
    * @brief Assigns path to controller
    * @param path Path received from action server
@@ -224,11 +234,12 @@ class ControllerServer : public nav2_util::LifecycleNode
 
   // Progress Checker Plugin
   pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
-  nav2_core::ProgressChecker::Ptr progress_checker_;
-  std::string default_progress_checker_id_;
-  std::string default_progress_checker_type_;
-  std::string progress_checker_id_;
-  std::string progress_checker_type_;
+  ProgressCheckerMap progress_checkers_;
+  std::vector<std::string> default_progress_checker_ids_;
+  std::vector<std::string> default_progress_checker_types_;
+  std::vector<std::string> progress_checker_ids_;
+  std::vector<std::string> progress_checker_types_;
+  std::string progress_checker_ids_concat_, current_progress_checker_;
 
   // Goal Checker Plugin
   pluginlib::ClassLoader<nav2_core::GoalChecker> goal_checker_loader_;

nav2_controller/src/controller_server.cpp

@@ -37,8 +37,8 @@ namespace nav2_controller
 ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
 : nav2_util::LifecycleNode("controller_server", "", options),
   progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"),
-  default_progress_checker_id_{"progress_checker"},
-  default_progress_checker_type_{"nav2_controller::SimpleProgressChecker"},
+  default_progress_checker_ids_{"progress_checker"},
+  default_progress_checker_types_{"nav2_controller::SimpleProgressChecker"},
   goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"),
   default_goal_checker_ids_{"goal_checker"},
   default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"},
@@ -50,7 +50,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
 
   declare_parameter("controller_frequency", 20.0);
 
-  declare_parameter("progress_checker_plugin", default_progress_checker_id_);
+  declare_parameter("progress_checker_plugins", default_progress_checker_ids_);
   declare_parameter("goal_checker_plugins", default_goal_checker_ids_);
   declare_parameter("controller_plugins", default_ids_);
   declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001));
@@ -69,7 +69,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
 
 ControllerServer::~ControllerServer()
 {
-  progress_checker_.reset();
+  progress_checkers_.clear();
   goal_checkers_.clear();
   controllers_.clear();
   costmap_thread_.reset();
@@ -82,11 +82,28 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
   RCLCPP_INFO(get_logger(), "Configuring controller interface");
 
-  get_parameter("progress_checker_plugin", progress_checker_id_);
-  if (progress_checker_id_ == default_progress_checker_id_) {
+  RCLCPP_INFO(get_logger(), "getting progress checker plugins..");
+  get_parameter("progress_checker_plugins", progress_checker_ids_);
+  try {
     nav2_util::declare_parameter_if_not_declared(
-      node, default_progress_checker_id_ + ".plugin",
-      rclcpp::ParameterValue(default_progress_checker_type_));
+      node, "progress_checker_plugin", rclcpp::PARAMETER_STRING);
+    std::string progress_checker_plugin;
+    progress_checker_plugin = node->get_parameter("progress_checker_plugin").as_string();
+    progress_checker_ids_.clear();
+    progress_checker_ids_.push_back(progress_checker_plugin);
+    RCLCPP_WARN(
+      get_logger(),
+      "\"progress_checker_plugin\" parameter was deprecated and will be removed soon. Use "
+      "\"progress_checker_plugins\" instead to specify a list of plugins");
+  } catch (const std::exception &) {
+    // This is normal situation: progress_checker_plugin parameter should not being declared
+  }
+  if (progress_checker_ids_ == default_progress_checker_ids_) {
+    for (size_t i = 0; i < default_progress_checker_ids_.size(); ++i) {
+      nav2_util::declare_parameter_if_not_declared(
+        node, default_progress_checker_ids_[i] + ".plugin",
+        rclcpp::ParameterValue(default_progress_checker_types_[i]));
+    }
   }
 
   RCLCPP_INFO(get_logger(), "getting goal checker plugins..");
@@ -110,6 +127,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
   controller_types_.resize(controller_ids_.size());
   goal_checker_types_.resize(goal_checker_ids_.size());
+  progress_checker_types_.resize(progress_checker_ids_.size());
 
   get_parameter("controller_frequency", controller_frequency_);
   get_parameter("min_x_velocity_threshold", min_x_velocity_threshold_);
@@ -125,20 +143,33 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   // Launch a thread to run the costmap node
   costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
 
-  try {
-    progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_);
-    progress_checker_ = progress_checker_loader_.createUniqueInstance(progress_checker_type_);
-    RCLCPP_INFO(
-      get_logger(), "Created progress_checker : %s of type %s",
-      progress_checker_id_.c_str(), progress_checker_type_.c_str());
-    progress_checker_->initialize(node, progress_checker_id_);
-  } catch (const pluginlib::PluginlibException & ex) {
-    RCLCPP_FATAL(
-      get_logger(),
-      "Failed to create progress_checker. Exception: %s", ex.what());
-    return nav2_util::CallbackReturn::FAILURE;
+  for (size_t i = 0; i != progress_checker_ids_.size(); i++) {
+    try {
+      progress_checker_types_[i] = nav2_util::get_plugin_type_param(
+        node, progress_checker_ids_[i]);
+      nav2_core::ProgressChecker::Ptr progress_checker =
+        progress_checker_loader_.createUniqueInstance(progress_checker_types_[i]);
+      RCLCPP_INFO(
+        get_logger(), "Created progress_checker : %s of type %s",
+        progress_checker_ids_[i].c_str(), progress_checker_types_[i].c_str());
+      progress_checker->initialize(node, progress_checker_ids_[i]);
+      progress_checkers_.insert({progress_checker_ids_[i], progress_checker});
+    } catch (const pluginlib::PluginlibException & ex) {
+      RCLCPP_FATAL(
+        get_logger(),
+        "Failed to create progress_checker. Exception: %s", ex.what());
+      return nav2_util::CallbackReturn::FAILURE;
+    }
+  }
+
+  for (size_t i = 0; i != progress_checker_ids_.size(); i++) {
+    progress_checker_ids_concat_ += progress_checker_ids_[i] + std::string(" ");
   }
 
+  RCLCPP_INFO(
+    get_logger(),
+    "Controller Server has %s progress checkers available.", progress_checker_ids_concat_.c_str());
+
   for (size_t i = 0; i != goal_checker_ids_.size(); i++) {
     try {
       goal_checker_types_[i] = nav2_util::get_plugin_type_param(node, goal_checker_ids_[i]);
@@ -284,6 +315,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   controllers_.clear();
 
   goal_checkers_.clear();
+  progress_checkers_.clear();
   if (costmap_ros_->get_current_state().id() ==
     lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
@@ -359,6 +391,32 @@ bool ControllerServer::findGoalCheckerId(
   return true;
 }
 
+bool ControllerServer::findProgressCheckerId(
+  const std::string & c_name,
+  std::string & current_progress_checker)
+{
+  if (progress_checkers_.find(c_name) == progress_checkers_.end()) {
+    if (progress_checkers_.size() == 1 && c_name.empty()) {
+      RCLCPP_WARN_ONCE(
+        get_logger(), "No progress checker was specified in parameter 'current_progress_checker'."
+        " Server will use only plugin loaded %s. "
+        "This warning will appear once.", progress_checker_ids_concat_.c_str());
+      current_progress_checker = progress_checkers_.begin()->first;
+    } else {
+      RCLCPP_ERROR(
+        get_logger(), "FollowPath called with progress_checker name %s in parameter"
+        " 'current_progress_checker', which does not exist. Available progress checkers are: %s.",
+        c_name.c_str(), progress_checker_ids_concat_.c_str());
+      return false;
+    }
+  } else {
+    RCLCPP_DEBUG(get_logger(), "Selected progress checker: %s.", c_name.c_str());
+    current_progress_checker = c_name;
+  }
+
+  return true;
+}
+
 void ControllerServer::computeControl()
 {
   std::lock_guard<std::mutex> lock(dynamic_params_lock_);
@@ -382,8 +440,16 @@ void ControllerServer::computeControl()
       throw nav2_core::ControllerException("Failed to find goal checker name: " + gc_name);
     }
 
+    std::string pc_name = action_server_->get_current_goal()->progress_checker_id;
+    std::string current_progress_checker;
+    if (findProgressCheckerId(pc_name, current_progress_checker)) {
+      current_progress_checker_ = current_progress_checker;
+    } else {
+      throw nav2_core::ControllerException("Failed to find progress checker name: " + pc_name);
+    }
+
     setPlannerPath(action_server_->get_current_goal()->path);
-    progress_checker_->reset();
+    progress_checkers_[current_progress_checker_]->reset();
 
     last_valid_cmd_time_ = now();
     rclcpp::WallRate loop_rate(controller_frequency_);
@@ -516,7 +582,7 @@ void ControllerServer::computeAndPublishVelocity()
     throw nav2_core::ControllerTFError("Failed to obtain robot pose");
   }
 
-  if (!progress_checker_->check(pose)) {
+  if (!progress_checkers_[current_progress_checker_]->check(pose)) {
     throw nav2_core::FailedToMakeProgress("Failed to make progress");
   }
 
@@ -607,6 +673,22 @@ void ControllerServer::updateGlobalPath()
       action_server_->terminate_current();
       return;
     }
+    std::string current_progress_checker;
+    if (findProgressCheckerId(goal->progress_checker_id, current_progress_checker)) {
+      if (current_progress_checker_ != current_progress_checker) {
+        RCLCPP_INFO(
+          get_logger(), "Change of progress checker %s requested, resetting it",
+          goal->progress_checker_id.c_str());
+        current_progress_checker_ = current_progress_checker;
+        progress_checkers_[current_progress_checker_]->reset();
+      }
+    } else {
+      RCLCPP_INFO(
+        get_logger(), "Terminating action, invalid progress checker %s requested.",
+        goal->progress_checker_id.c_str());
+      action_server_->terminate_current();
+      return;
+    }
     setPlannerPath(goal->path);
   }
 }

nav2_msgs/action/FollowPath.action

@@ -13,6 +13,7 @@ uint16 NO_VALID_CONTROL=106
 nav_msgs/Path path
 string controller_id
 string goal_checker_id
+string progress_checker_id
 ---
 #result definition
 std_msgs/Empty result

nav2_regulated_pure_pursuit_controller/README.md

@@ -88,7 +88,7 @@ controller_server:
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
-    progress_checker_plugin: "progress_checker"
+    progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: "goal_checker"
     controller_plugins: ["FollowPath"]
 

nav2_rotation_shim_controller/README.md

@@ -45,7 +45,7 @@ controller_server:
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
-    progress_checker_plugin: "progress_checker"
+    progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: "goal_checker"
     controller_plugins: ["FollowPath"]
 

nav2_system_tests/src/costmap_filters/keepout_params.yaml

@@ -99,7 +99,7 @@ controller_server:
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
-    progress_checker_plugin: "progress_checker"
+    progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: ["goal_checker"]
     controller_plugins: ["FollowPath"]
 

nav2_system_tests/src/costmap_filters/speed_global_params.yaml

@@ -101,7 +101,7 @@ controller_server:
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
     speed_limit_topic: "/speed_limit"
-    progress_checker_plugin: "progress_checker"
+    progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: ["goal_checker"]
     controller_plugins: ["FollowPath"]
 

nav2_system_tests/src/costmap_filters/speed_local_params.yaml

@@ -101,7 +101,7 @@ controller_server:
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
     speed_limit_topic: "/speed_limit"
-    progress_checker_plugin: "progress_checker"
+    progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: ["goal_checker"]
     controller_plugins: ["FollowPath"]