add multiple progress checker plugins

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

@@ -65,6 +65,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", ""),
       });
   }
 };

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -93,6 +93,7 @@
       <input_port name="controller_id" default="FollowPath"/>
       <input_port name="path">Path to follow</input_port>
       <input_port name="goal_checker_id" default="GoalChecker">Goal checker</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>
     </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);
 }
 
 void FollowPathAction::on_wait_for_result(
@@ -64,6 +65,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_controller/include/nav2_controller/controller_server.hpp

@@ -50,7 +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
    * @param options Additional options to control creation of the node.
@@ -142,6 +142,14 @@ 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 +232,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

@@ -36,8 +36,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"},
@@ -49,7 +49,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));
@@ -70,7 +70,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
 
 ControllerServer::~ControllerServer()
 {
-  progress_checker_.reset();
+  progress_checkers_.clear();
   goal_checkers_.clear();
   controllers_.clear();
   costmap_thread_.reset();
@@ -80,16 +80,30 @@ nav2_util::CallbackReturn
 ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
 {
   auto node = shared_from_this();
-
   RCLCPP_INFO(get_logger(), "Configuring controller interface");
-
-  get_parameter("progress_checker_plugin", progress_checker_id_);
-  if (progress_checker_id_ == default_progress_checker_id_) {
-    nav2_util::declare_parameter_if_not_declared(
-      node, default_progress_checker_id_ + ".plugin",
-      rclcpp::ParameterValue(default_progress_checker_type_));
+  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, "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..");
   get_parameter("goal_checker_plugins", goal_checker_ids_);
   if (goal_checker_ids_ == default_goal_checker_ids_) {
@@ -111,6 +125,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_);
@@ -124,20 +139,33 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
 
   costmap_ros_->on_configure(state);
 
-  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]);
@@ -271,6 +299,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state)
   controllers_.clear();
 
   goal_checkers_.clear();
+  progress_checkers_.clear();
   costmap_ros_->on_cleanup(state);
 
   // Release any allocated resources
@@ -341,6 +370,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_);
@@ -366,8 +421,17 @@ void ControllerServer::computeControl()
       return;
     }
 
+    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 {
+      action_server_->terminate_current();
+      return;
+    }
+
     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_);
@@ -449,7 +513,7 @@ void ControllerServer::computeAndPublishVelocity()
     throw nav2_core::PlannerException("Failed to obtain robot pose");
   }
 
-  if (!progress_checker_->check(pose)) {
+ if (!progress_checkers_[current_progress_checker_]->check(pose)) {
     throw nav2_core::PlannerException("Failed to make progress");
   }
 
@@ -528,6 +592,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

@@ -2,6 +2,7 @@
 nav_msgs/Path path
 string controller_id
 string goal_checker_id
+string progress_checker_id
 ---
 #result definition
 std_msgs/Empty result