From 152204d4e69bb37bff393a35fc134e4d3764aeea Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke Date: Thu, 15 May 2025 09:42:08 +0200 Subject: [PATCH 1/9] Add DynamicParameterPatterns to pose_pogress_checker plugin Signed-off-by: Nils-Christian Iseke --- .../plugins/pose_progress_checker.hpp | 51 +++++++-- .../plugins/pose_progress_checker.cpp | 106 +++++++++++++----- 2 files changed, 120 insertions(+), 37 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp index 48eb580c964..7a53e41a2db 100644 --- a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_controller/plugins/simple_progress_checker.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -31,6 +32,41 @@ namespace nav2_controller class PoseProgressChecker : public SimpleProgressChecker { + struct Parameters + { + double required_movement_angle; + }; + +/** + * @class nav2_controller::PoseProgressChecker::ParameterHandler + * @brief This class handls parameters and dynamic parameter updates for the nav2_controller. + */ + class ParameterHandler + { +public: + ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, rclcpp::Logger & logger); + ~ParameterHandler(); + Parameters * getParams() {return ¶ms_;} + +protected: + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + + void + updateParametersCallback( + std::vector parameters); + + rcl_interfaces::msg::SetParametersResult + validateParameterUpdatesCallback( + std::vector parameters); + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_; + Parameters params_; + std::string plugin_name_; + rclcpp::Logger logger_ {rclcpp::get_logger("PoseProgressChecker")}; + }; + public: void initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, @@ -50,18 +86,15 @@ class PoseProgressChecker : public SimpleProgressChecker const geometry_msgs::msg::Pose2D &); double required_movement_angle_; - - // Dynamic parameters handler - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + Parameters * params_; std::string plugin_name_; - /** - * @brief Callback executed when a parameter change is detected - * @param parameters list of changed parameters - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); + rclcpp::Logger logger_ {rclcpp::get_logger("PoseProgressChecker")}; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + std::unique_ptr param_handler_; }; + + } // namespace nav2_controller #endif // NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_ diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp index 271e5635c07..8a9a950e1f8 100644 --- a/nav2_controller/plugins/pose_progress_checker.cpp +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -12,39 +12,106 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_controller/plugins/pose_progress_checker.hpp" #include #include #include #include + #include "angles/angles.h" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_util/node_utils.hpp" #include "pluginlib/class_list_macros.hpp" +#include "nav2_controller/plugins/pose_progress_checker.hpp" +#include "nav2_core/controller_exceptions.hpp" using rcl_interfaces::msg::ParameterType; using std::placeholders::_1; namespace nav2_controller { +using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; + + +PoseProgressChecker::ParameterHandler::ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, rclcpp::Logger & logger) +{ + node_ = node; + plugin_name_ = plugin_name; + logger_ = logger; + + declare_parameter_if_not_declared(node, plugin_name + ".required_movement_angle", + rclcpp::ParameterValue(0.5)); + node->get_parameter(plugin_name + ".required_movement_angle", + params_.required_movement_angle); + on_set_params_handler_ = node->add_on_set_parameters_callback( + [this](const auto & params) { + return this->validateParameterUpdatesCallback(params); + }); + post_set_params_handler_ = node->add_post_set_parameters_callback( + [this](const auto & params) { + return this->updateParametersCallback(params); + }); +} +PoseProgressChecker::ParameterHandler::~ParameterHandler() = default; + +void PoseProgressChecker::ParameterHandler::updateParametersCallback( + std::vector parameters) +{ + for (const auto & param : parameters) { + const auto & name = param.get_name(); + const auto & type = param.get_type(); + if (name == plugin_name_ + ".required_movement_angle" && + type == ParameterType::PARAMETER_DOUBLE) + { + params_.required_movement_angle = param.as_double(); + } + } +} +rcl_interfaces::msg::SetParametersResult +PoseProgressChecker::ParameterHandler::validateParameterUpdatesCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + { + if (name.find(plugin_name_ + ".") != 0) { + continue; + } + if (name == plugin_name_ + ".required_movement_angle" && + type == ParameterType::PARAMETER_DOUBLE && + (parameter.as_double() <= 0.0 || parameter.as_double() >= 2 * M_PI)) + { + result.successful = false; + result.reason = "The value required_movement_angle is incorrectly set, " + "it should be 0 < required_movement_angle < 2PI. Ignoring parameter update."; + return result; + } + } + } + result.successful = true; + return result; +} void PoseProgressChecker::initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) { + auto node = parent.lock(); + if (!node) { + throw nav2_core::ControllerException("Unable to lock node!"); + } + node_ = parent; plugin_name_ = plugin_name; + logger_ = node->get_logger(); + param_handler_ = std::make_unique(node, plugin_name_, logger_); + params_ = param_handler_->getParams(); SimpleProgressChecker::initialize(parent, plugin_name); - auto node = parent.lock(); - - nav2_util::declare_parameter_if_not_declared( - node, plugin_name + ".required_movement_angle", rclcpp::ParameterValue(0.5)); - node->get_parameter_or(plugin_name + ".required_movement_angle", required_movement_angle_, 0.5); - - // Add callback for dynamic parameters - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind(&PoseProgressChecker::dynamicParametersCallback, this, _1)); } bool PoseProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) @@ -64,7 +131,7 @@ bool PoseProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) bool PoseProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose) { return pose_distance(pose, baseline_pose_) > radius_ || - poseAngleDistance(pose, baseline_pose_) > required_movement_angle_; + poseAngleDistance(pose, baseline_pose_) > params_->required_movement_angle; } double PoseProgressChecker::poseAngleDistance( @@ -74,23 +141,6 @@ double PoseProgressChecker::poseAngleDistance( return abs(angles::shortest_angular_distance(pose1.theta, pose2.theta)); } -rcl_interfaces::msg::SetParametersResult -PoseProgressChecker::dynamicParametersCallback(std::vector parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".required_movement_angle") { - required_movement_angle_ = parameter.as_double(); - } - } - } - result.successful = true; - return result; -} } // namespace nav2_controller From 69fbcbbf7042f62f72ad911f689641f9179f3199 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke Date: Thu, 15 May 2025 09:52:44 +0200 Subject: [PATCH 2/9] ~ParameterHandler Signed-off-by: Nils-Christian Iseke --- nav2_controller/plugins/pose_progress_checker.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp index 8a9a950e1f8..eb634453f2c 100644 --- a/nav2_controller/plugins/pose_progress_checker.cpp +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -56,8 +56,18 @@ PoseProgressChecker::ParameterHandler::ParameterHandler( return this->updateParametersCallback(params); }); } -PoseProgressChecker::ParameterHandler::~ParameterHandler() = default; - +PoseProgressChecker::ParameterHandler::~ParameterHandler() +{ + auto node = node_.lock(); + if (post_set_params_handler_ && node) { + node->remove_post_set_parameters_callback(post_set_params_handler_.get()); + } + post_set_params_handler_.reset(); + if (on_set_params_handler_ && node) { + node->remove_on_set_parameters_callback(on_set_params_handler_.get()); + } + on_set_params_handler_.reset(); +} void PoseProgressChecker::ParameterHandler::updateParametersCallback( std::vector parameters) { From b40d2adf45ca2e0382011231bdf1e02a90abc9fe Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Date: Mon, 19 May 2025 09:10:13 +0200 Subject: [PATCH 3/9] Apply suggestions from code review Co-authored-by: Steve Macenski Signed-off-by: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> --- .../nav2_controller/plugins/pose_progress_checker.hpp | 5 ++--- nav2_controller/plugins/pose_progress_checker.cpp | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp index 7a53e41a2db..81925732e28 100644 --- a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp @@ -43,14 +43,14 @@ class PoseProgressChecker : public SimpleProgressChecker */ class ParameterHandler { -public: + public: ParameterHandler( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name, rclcpp::Logger & logger); ~ParameterHandler(); Parameters * getParams() {return ¶ms_;} -protected: + protected: rclcpp_lifecycle::LifecycleNode::WeakPtr node_; void @@ -94,7 +94,6 @@ class PoseProgressChecker : public SimpleProgressChecker std::unique_ptr param_handler_; }; - } // namespace nav2_controller #endif // NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_ diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp index eb634453f2c..9ed042178e7 100644 --- a/nav2_controller/plugins/pose_progress_checker.cpp +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -31,10 +31,10 @@ using std::placeholders::_1; namespace nav2_controller { + using nav2_util::declare_parameter_if_not_declared; using rcl_interfaces::msg::ParameterType; - PoseProgressChecker::ParameterHandler::ParameterHandler( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name, rclcpp::Logger & logger) @@ -54,7 +54,7 @@ PoseProgressChecker::ParameterHandler::ParameterHandler( post_set_params_handler_ = node->add_post_set_parameters_callback( [this](const auto & params) { return this->updateParametersCallback(params); - }); + }); } PoseProgressChecker::ParameterHandler::~ParameterHandler() { From 13bbe0773fd31801e219779bdb32cd68ac4581c2 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Date: Mon, 19 May 2025 09:12:09 +0200 Subject: [PATCH 4/9] Place Validate before update Signed-off-by: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> --- .../plugins/pose_progress_checker.cpp | 27 +++++++++---------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp index 9ed042178e7..b9d1a316f00 100644 --- a/nav2_controller/plugins/pose_progress_checker.cpp +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -68,19 +68,6 @@ PoseProgressChecker::ParameterHandler::~ParameterHandler() } on_set_params_handler_.reset(); } -void PoseProgressChecker::ParameterHandler::updateParametersCallback( - std::vector parameters) -{ - for (const auto & param : parameters) { - const auto & name = param.get_name(); - const auto & type = param.get_type(); - if (name == plugin_name_ + ".required_movement_angle" && - type == ParameterType::PARAMETER_DOUBLE) - { - params_.required_movement_angle = param.as_double(); - } - } -} rcl_interfaces::msg::SetParametersResult PoseProgressChecker::ParameterHandler::validateParameterUpdatesCallback( std::vector parameters) @@ -107,7 +94,19 @@ PoseProgressChecker::ParameterHandler::validateParameterUpdatesCallback( result.successful = true; return result; } - +void PoseProgressChecker::ParameterHandler::updateParametersCallback( + std::vector parameters) +{ + for (const auto & param : parameters) { + const auto & name = param.get_name(); + const auto & type = param.get_type(); + if (name == plugin_name_ + ".required_movement_angle" && + type == ParameterType::PARAMETER_DOUBLE) + { + params_.required_movement_angle = param.as_double(); + } + } +} void PoseProgressChecker::initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) From 9fd963859b1a48c9ca2e9cb290c872609e22e521 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke Date: Mon, 19 May 2025 11:03:23 +0200 Subject: [PATCH 5/9] Uncrustify Signed-off-by: Nils-Christian Iseke --- .../include/nav2_controller/plugins/pose_progress_checker.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp index 81925732e28..28cac2a942e 100644 --- a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp @@ -43,14 +43,14 @@ class PoseProgressChecker : public SimpleProgressChecker */ class ParameterHandler { - public: +public: ParameterHandler( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name, rclcpp::Logger & logger); ~ParameterHandler(); Parameters * getParams() {return ¶ms_;} - protected: +protected: rclcpp_lifecycle::LifecycleNode::WeakPtr node_; void From 04fa97f41b5ab62ca54b3ee7254af2709bf20b4f Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke Date: Thu, 22 May 2025 09:44:58 +0000 Subject: [PATCH 6/9] Use a shared ptr for params Signed-off-by: Nils-Christian Iseke --- .../include/nav2_controller/plugins/pose_progress_checker.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp index 28cac2a942e..7d5f93febec 100644 --- a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp @@ -48,7 +48,7 @@ class PoseProgressChecker : public SimpleProgressChecker rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name, rclcpp::Logger & logger); ~ParameterHandler(); - Parameters * getParams() {return ¶ms_;} + std::shared_ptr getParams() {return std::make_shared(params_);} protected: rclcpp_lifecycle::LifecycleNode::WeakPtr node_; @@ -86,7 +86,7 @@ class PoseProgressChecker : public SimpleProgressChecker const geometry_msgs::msg::Pose2D &); double required_movement_angle_; - Parameters * params_; + std::shared_ptr params_; std::string plugin_name_; rclcpp::Logger logger_ {rclcpp::get_logger("PoseProgressChecker")}; From 9a1ccbbdba218d43b6be94392984594444dcb712 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke Date: Thu, 22 May 2025 15:29:34 +0000 Subject: [PATCH 7/9] Const reference Signed-off-by: Nils-Christian Iseke --- .../include/nav2_controller/plugins/pose_progress_checker.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp index 7d5f93febec..d3dc035b429 100644 --- a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp @@ -48,7 +48,7 @@ class PoseProgressChecker : public SimpleProgressChecker rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name, rclcpp::Logger & logger); ~ParameterHandler(); - std::shared_ptr getParams() {return std::make_shared(params_);} + const Parameters & getParams() const {return params_;} protected: rclcpp_lifecycle::LifecycleNode::WeakPtr node_; @@ -86,7 +86,7 @@ class PoseProgressChecker : public SimpleProgressChecker const geometry_msgs::msg::Pose2D &); double required_movement_angle_; - std::shared_ptr params_; + std::shared_ptr params_; std::string plugin_name_; rclcpp::Logger logger_ {rclcpp::get_logger("PoseProgressChecker")}; From a1a80feee6eab2266d2e9bf9391c509eddf33320 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke Date: Thu, 22 May 2025 15:43:06 +0000 Subject: [PATCH 8/9] Unique ptr. Signed-off-by: Nils-Christian Iseke --- .../include/nav2_controller/plugins/pose_progress_checker.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp index d3dc035b429..d9d0c396d08 100644 --- a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp @@ -86,7 +86,7 @@ class PoseProgressChecker : public SimpleProgressChecker const geometry_msgs::msg::Pose2D &); double required_movement_angle_; - std::shared_ptr params_; + std::unique_ptr params_; std::string plugin_name_; rclcpp::Logger logger_ {rclcpp::get_logger("PoseProgressChecker")}; From 5a909a6bbd78f1e38637920023aaedf71750ba06 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke Date: Fri, 23 May 2025 07:11:37 +0000 Subject: [PATCH 9/9] revert pr commit Signed-off-by: Nils-Christian Iseke --- .../include/nav2_controller/plugins/pose_progress_checker.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp index d9d0c396d08..7d5f93febec 100644 --- a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp @@ -48,7 +48,7 @@ class PoseProgressChecker : public SimpleProgressChecker rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name, rclcpp::Logger & logger); ~ParameterHandler(); - const Parameters & getParams() const {return params_;} + std::shared_ptr getParams() {return std::make_shared(params_);} protected: rclcpp_lifecycle::LifecycleNode::WeakPtr node_; @@ -86,7 +86,7 @@ class PoseProgressChecker : public SimpleProgressChecker const geometry_msgs::msg::Pose2D &); double required_movement_angle_; - std::unique_ptr params_; + std::shared_ptr params_; std::string plugin_name_; rclcpp::Logger logger_ {rclcpp::get_logger("PoseProgressChecker")};