From 16ae071bc931fd6befd3d15d2a44a04fad9a20c4 Mon Sep 17 00:00:00 2001 From: Enzo Ghisoni <33607172+EnzoGhisoni@users.noreply.github.com> Date: Wed, 14 Aug 2024 03:37:06 +0200 Subject: [PATCH] nav2_collision_monitor dynamic parameters polygon and source enabled for Humble (#4615) * Copy modification from c2d84dfe671077bd76781ea356141b6d7ebb86f4 into humble collision_monitor for dynamic parameter enabled in polygon * Add the enabled dynamic parameter for source. Signed-off-by: Enzo Ghisoni * Start backport action_state_ declaration in collision_monitor_node_test.cpp Signed-off-by: EnzoGhisoni --------- Signed-off-by: EnzoGhisoni Co-authored-by: EnzoGhisoni --- nav2_collision_monitor/CMakeLists.txt | 2 + .../nav2_collision_monitor/polygon.hpp | 19 ++++++++- .../include/nav2_collision_monitor/source.hpp | 23 +++++++++++ .../params/collision_monitor_params.yaml | 5 +++ .../src/collision_monitor_node.cpp | 11 ++++- nav2_collision_monitor/src/pointcloud.cpp | 1 + nav2_collision_monitor/src/polygon.cpp | 34 ++++++++++++++++ nav2_collision_monitor/src/range.cpp | 1 + nav2_collision_monitor/src/scan.cpp | 1 + nav2_collision_monitor/src/source.cpp | 40 +++++++++++++++++++ nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/msg/CollisionMonitorState.msg | 9 +++++ 12 files changed, 144 insertions(+), 3 deletions(-) create mode 100644 nav2_msgs/msg/CollisionMonitorState.msg diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 546d080790e..35cba4a2c94 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) ### Header ### @@ -35,6 +36,7 @@ set(dependencies tf2_geometry_msgs nav2_util nav2_costmap_2d + nav2_msgs ) set(monitor_executable_name collision_monitor) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 97423dc411d..d5843dd606c 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -86,6 +86,11 @@ class Polygon * @return Action type for current polygon */ ActionType getActionType() const; + /** + * @brief Obtains polygon enabled state + * @return Whether polygon is enabled + */ + bool getEnabled() const; /** * @brief Obtains polygon maximum points to enter inside polygon causing no action * @return Maximum points to enter to current polygon and take no action @@ -161,6 +166,14 @@ class Polygon * @param point Given point to check * @return True if given point is inside polygon, otherwise false */ + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + bool isPointInside(const Point & point) const; // ----- Variables ----- @@ -169,7 +182,9 @@ 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 std::string polygon_name_; @@ -185,6 +200,8 @@ class Polygon double simulation_time_step_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; + /// @brief Whether polygon is enabled + bool enabled_; // Global variables /// @brief TF buffer diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 8bc750cc717..30d58d53bc1 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -74,7 +74,19 @@ class Source const rclcpp::Time & curr_time, std::vector & data) const = 0; + /** + * @brief Obtains source enabled state + * @return Whether source is enabled + */ + bool getEnabled() const; + protected: + /** + * @brief Source configuration routine. + * @return True in case of everything is configured correctly, or false otherwise + */ + bool configure(); + /** * @brief Supporting routine obtaining ROS-parameters common for all data sources * @param source_topic Output name of source subscription topic @@ -91,12 +103,21 @@ class Source const rclcpp::Time & source_time, const rclcpp::Time & curr_time) const; + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + // ----- Variables ----- /// @brief Collision Monitor node 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 data source @@ -116,6 +137,8 @@ class Source /// @brief Whether to correct source data towards to base frame movement, /// considering the difference between current time and latest source time bool base_shift_correction_; + /// @brief Whether source is enabled + bool enabled_; }; // class Source } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 1b0c36529e7..a84d74c6590 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -22,6 +22,7 @@ collision_monitor: max_points: 3 visualize: True polygon_pub_topic: "polygon_stop" + enabled: True PolygonSlow: type: "polygon" points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4] @@ -30,6 +31,7 @@ collision_monitor: slowdown_ratio: 0.3 visualize: True polygon_pub_topic: "polygon_slowdown" + enabled: True FootprintApproach: type: "polygon" action_type: "approach" @@ -38,12 +40,15 @@ collision_monitor: simulation_time_step: 0.1 max_points: 5 visualize: False + enabled: True observation_sources: ["scan"] scan: type: "scan" topic: "/scan" + enabled: True pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" min_height: 0.1 max_height: 0.5 + enabled: True diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 8e9914a400f..390220d8fee 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -356,7 +356,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) // Fill collision_points array from different data sources for (std::shared_ptr source : sources_) { - source->getData(curr_time, collision_points); + if (source->getEnabled()) { + source->getData(curr_time, collision_points); + } } // By default - there is no action @@ -365,6 +367,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) std::shared_ptr action_polygon; for (std::shared_ptr polygon : polygons_) { + if (!polygon->getEnabled()) { + continue; + } if (robot_action.action_type == STOP) { // If robot already should stop, do nothing break; @@ -482,7 +487,9 @@ void CollisionMonitor::printAction( void CollisionMonitor::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - polygon->publish(); + if (polygon->getEnabled()) { + polygon->publish(); + } } } diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index d4d2ea1adff..7e84d8890a0 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -48,6 +48,7 @@ PointCloud::~PointCloud() void PointCloud::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index fcbe9312e1b..b756c98afce 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -44,6 +44,7 @@ Polygon::~Polygon() { RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str()); poly_.clear(); + dyn_params_handler_.reset(); } bool Polygon::configure() @@ -82,6 +83,10 @@ bool Polygon::configure() polygon_pub_topic, polygon_qos); } + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Polygon::dynamicParametersCallback, this, std::placeholders::_1)); + return true; } @@ -109,6 +114,11 @@ ActionType Polygon::getActionType() const return action_type_; } +bool Polygon::getEnabled() const +{ + return enabled_; +} + int Polygon::getMaxPoints() const { return max_points_; @@ -246,6 +256,10 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) return false; } + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(polygon_name_ + ".enabled").as_bool(); + 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(); @@ -352,6 +366,26 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp return true; } +rcl_interfaces::msg::SetParametersResult +Polygon::dynamicParametersCallback( + std::vector parameters) +{ + 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 == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == polygon_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + 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." diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index ae15bd488cb..8f750490962 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -48,6 +48,7 @@ Range::~Range() void Range::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 50f670cb131..8ac45904a21 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -45,6 +45,7 @@ Scan::~Scan() void Scan::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index 6ad41224b5a..41b6f647ee8 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -46,6 +46,17 @@ Source::~Source() { } +bool Source::configure() +{ + auto node = node_.lock(); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Source::dynamicParametersCallback, this, std::placeholders::_1)); + + return true; +} + void Source::getCommonParameters(std::string & source_topic) { auto node = node_.lock(); @@ -57,6 +68,10 @@ void Source::getCommonParameters(std::string & source_topic) node, source_name_ + ".topic", rclcpp::ParameterValue("scan")); // Set deafult topic for laser scanner source_topic = node->get_parameter(source_name_ + ".topic").as_string(); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(source_name_ + ".enabled").as_bool(); } bool Source::sourceValid( @@ -78,4 +93,29 @@ bool Source::sourceValid( return true; } +bool Source::getEnabled() const +{ + return enabled_; +} + +rcl_interfaces::msg::SetParametersResult +Source::dynamicParametersCallback( + std::vector parameters) +{ + 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 == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == source_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + } // namespace nav2_collision_monitor diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 2aa446f47fe..4e52c24c6b8 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -14,6 +14,7 @@ nav2_package() rosidl_generate_interfaces(${PROJECT_NAME} "msg/CollisionDetectorState.msg" + "msg/CollisionMonitorState.msg" "msg/Costmap.msg" "msg/CostmapMetaData.msg" "msg/CostmapFilterInfo.msg" diff --git a/nav2_msgs/msg/CollisionMonitorState.msg b/nav2_msgs/msg/CollisionMonitorState.msg new file mode 100644 index 00000000000..e470df57eb3 --- /dev/null +++ b/nav2_msgs/msg/CollisionMonitorState.msg @@ -0,0 +1,9 @@ +# Action type for robot in Collision Monitor +uint8 DO_NOTHING=0 # No action +uint8 STOP=1 # Stop the robot +uint8 SLOWDOWN=2 # Slowdown in percentage from current operating speed +uint8 APPROACH=3 # Keep constant time interval before collision +uint8 action_type + +# Name of triggered polygon +string polygon_name \ No newline at end of file