From 80b71b60d476e9f89302ccbedae143a0deee20f3 Mon Sep 17 00:00:00 2001 From: Stevedan Date: Wed, 27 Sep 2023 18:16:35 +0200 Subject: [PATCH 1/5] add multiple progress checker plugins --- .../nav2_controller/controller_server.hpp | 21 ++- nav2_controller/src/controller_server.cpp | 132 ++++++++++++++---- 2 files changed, 120 insertions(+), 33 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 89d9a4822c..ceab12523e 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -50,7 +50,7 @@ class ControllerServer : public nav2_util::LifecycleNode public: using ControllerMap = std::unordered_map; using GoalCheckerMap = std::unordered_map; - + using ProgressCheckerMap = std::unordered_map; /** * @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 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 default_progress_checker_ids_; + std::vector default_progress_checker_types_; + std::vector progress_checker_ids_; + std::vector progress_checker_types_; + std::string progress_checker_ids_concat_, current_progress_checker_; // Goal Checker Plugin pluginlib::ClassLoader goal_checker_loader_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index a44caeda3d..490049ce5f 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/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_.reset(); 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.."); +try { + get_parameter("progress_checker_plugins", progress_checker_id_); + 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_) { @@ -124,20 +138,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 +298,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 +369,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 lock(dynamic_params_lock_); @@ -366,8 +420,16 @@ 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 { + 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_); @@ -449,8 +511,8 @@ void ControllerServer::computeAndPublishVelocity() throw nav2_core::PlannerException("Failed to obtain robot pose"); } - if (!progress_checker_->check(pose)) { - throw nav2_core::PlannerException("Failed to make progress"); + if (!progress_checkers_[current_progress_checker_]->check(pose)) { + throw nav2_core::FailedToMakeProgress("Failed to make progress"); } nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); @@ -528,6 +590,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); } } From 543f93d25937fff3aae5640154430115584fb883 Mon Sep 17 00:00:00 2001 From: Stevedan Date: Wed, 27 Sep 2023 18:46:13 +0200 Subject: [PATCH 2/5] update changes on follow path --- .../plugins/action/follow_path_action.hpp | 1 + nav2_behavior_tree/nav2_tree_nodes.xml | 1 + nav2_behavior_tree/plugins/action/follow_path_action.cpp | 9 +++++++++ nav2_controller/src/controller_server.cpp | 4 ++-- 4 files changed, 13 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index a97993c773..53057ffc42 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -65,6 +65,7 @@ class FollowPathAction : public BtActionNode BT::InputPort("path", "Path to follow"), BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), + BT::InputPort("progress_checker_id", ""), }); } }; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 72a09fa3eb..19079b2995 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -93,6 +93,7 @@ Path to follow Goal checker + Progress checker Service name Server timeout diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 0fc0dc5b57..8afb01827a 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/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 diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 490049ce5f..aac3690baf 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -70,7 +70,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) ControllerServer::~ControllerServer() { - progress_checkers_.reset(); + progress_checkers_.clear(); goal_checkers_.clear(); controllers_.clear(); costmap_thread_.reset(); @@ -83,7 +83,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_INFO(get_logger(), "Configuring controller interface"); RCLCPP_INFO(get_logger(), "getting progress checker plugins.."); try { - get_parameter("progress_checker_plugins", progress_checker_id_); + get_parameter("progress_checker_plugins", progress_checker_ids_); nav2_util::declare_parameter_if_not_declared( node, "progress_checker_plugin", rclcpp::PARAMETER_STRING); std::string progress_checker_plugin; From 47fd7f5b4a96238918a18d616269c09fb1f03a0d Mon Sep 17 00:00:00 2001 From: Stevedan Date: Wed, 27 Sep 2023 19:22:53 +0200 Subject: [PATCH 3/5] add progress checker --- nav2_msgs/action/FollowPath.action | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 5462faa239..e8333f1f00 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/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 From b6b16bf7cb36397cb5e06dc294b6887af0439094 Mon Sep 17 00:00:00 2001 From: Stevedan Date: Wed, 27 Sep 2023 19:26:21 +0200 Subject: [PATCH 4/5] fix minor compilation problems --- nav2_controller/src/controller_server.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index aac3690baf..0156efba90 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -425,7 +425,8 @@ void ControllerServer::computeControl() 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); + action_server_->terminate_current(); + return; } setPlannerPath(action_server_->get_current_goal()->path); @@ -512,7 +513,7 @@ void ControllerServer::computeAndPublishVelocity() } if (!progress_checkers_[current_progress_checker_]->check(pose)) { - throw nav2_core::FailedToMakeProgress("Failed to make progress"); + throw nav2_core::PlannerException("Failed to make progress"); } nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); From f71ac117e40bca140564a2516dc0bbec4eb88ff1 Mon Sep 17 00:00:00 2001 From: Stevedan Date: Wed, 27 Sep 2023 19:47:08 +0200 Subject: [PATCH 5/5] minor logic fixes --- nav2_controller/src/controller_server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 0156efba90..e2c89375f6 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -82,8 +82,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) auto node = shared_from_this(); RCLCPP_INFO(get_logger(), "Configuring controller interface"); RCLCPP_INFO(get_logger(), "getting progress checker plugins.."); -try { 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; @@ -125,6 +125,7 @@ try { 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_);