From 2c1eca14ef0e8280d30a732086fc38f661745fd6 Mon Sep 17 00:00:00 2001 From: HovorunB Date: Thu, 22 Jun 2023 16:28:20 +0200 Subject: [PATCH 1/2] Dynamically set max_points for collision monitor --- .../nav2_collision_monitor/polygon.hpp | 8 +++++ nav2_collision_monitor/src/polygon.cpp | 32 ++++++++++++++++++- 2 files changed, 39 insertions(+), 1 deletion(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 91edb82ff03..0965d31e7e8 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -162,6 +162,12 @@ class Polygon */ virtual bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic); + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + /** * @brief Checks if point is inside polygon * @param point Given point to check @@ -175,6 +181,8 @@ class Polygon nav2_util::LifecycleNode::WeakPtr node_; /// @brief Collision monitor node logger stored for further usage rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Basic parameters /// @brief Name of polygon diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 2f55734e85a..03f8ba47756 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -23,6 +23,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_collision_monitor/kinematics.hpp" +using rcl_interfaces::msg::ParameterType; namespace nav2_collision_monitor { @@ -44,6 +45,7 @@ Polygon::~Polygon() { RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str()); poly_.clear(); + dyn_params_handler_.reset(); } bool Polygon::configure() @@ -58,6 +60,12 @@ bool Polygon::configure() if (!getParameters(polygon_pub_topic, footprint_topic)) { return false; } + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &Polygon::dynamicParametersCallback, + this, + std::placeholders::_1) + ); if (!footprint_topic.empty()) { footprint_sub_ = std::make_unique( @@ -275,7 +283,7 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".max_points", rclcpp::ParameterValue(3)); - max_points_ = node->get_parameter(polygon_name_ + ".max_points").as_int(); + node->get_parameter(polygon_name_ + ".max_points", max_points_); if (action_type_ == SLOWDOWN) { nav2_util::declare_parameter_if_not_declared( @@ -388,6 +396,28 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp return true; } +rcl_interfaces::msg::SetParametersResult +Polygon::dynamicParametersCallback( + std::vector parameters) +{ + // std::lock_guard guard(*getMutex()); + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == polygon_name_ + "." + "max_points") { + max_points_ = parameter.as_int(); + RCLCPP_INFO(logger_, "SET MAX POINTS TO %i", max_points_); + } + } + } + result.successful = true; + return result; +} + inline bool Polygon::isPointInside(const Point & point) const { // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." From adc5f7bff11a36cf3fa67748875a685a43ca9b26 Mon Sep 17 00:00:00 2001 From: HovorunB Date: Thu, 22 Jun 2023 17:42:39 +0200 Subject: [PATCH 2/2] remove log --- nav2_collision_monitor/src/polygon.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 03f8ba47756..50782474be5 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -410,7 +410,6 @@ Polygon::dynamicParametersCallback( if (param_type == ParameterType::PARAMETER_INTEGER) { if (param_name == polygon_name_ + "." + "max_points") { max_points_ = parameter.as_int(); - RCLCPP_INFO(logger_, "SET MAX POINTS TO %i", max_points_); } } }