nav2_collision_monitor dynamic parameters polygon and source enabled for Humble (#4615)

* Copy modification from c2d84df into humble collision_monitor for dynamic parameter enabled in polygon

* Add the enabled dynamic parameter for source. Signed-off-by: Enzo Ghisoni <enzo.ghisoni@hotmail.fr>

* Start backport action_state_ declaration in collision_monitor_node_test.cpp

Signed-off-by: EnzoGhisoni <enzo.ghisoni@botronics.be>

---------

Signed-off-by: EnzoGhisoni <enzo.ghisoni@botronics.be>
Co-authored-by: EnzoGhisoni <enzo.ghisoni@botronics.be>

Author: EnzoGhisoni

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(executable_name collision_monitor)

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<rclcpp::Parameter> 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<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
+  /// @brief Whether polygon is enabled
+  bool enabled_;
 
   // Global variables
   /// @brief TF buffer

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp

@@ -74,7 +74,19 @@ class Source
     const rclcpp::Time & curr_time,
     std::vector<Point> & 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<rclcpp::Parameter> 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

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

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -355,7 +355,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
 
   // Fill collision_points array from different data sources
   for (std::shared_ptr<Source> source : sources_) {
-    source->getData(curr_time, collision_points);
+    if (source->getEnabled()) {
+      source->getData(curr_time, collision_points);
+    }
   }
 
   // By default - there is no action
@@ -364,6 +366,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
   std::shared_ptr<Polygon> action_polygon;
 
   for (std::shared_ptr<Polygon> polygon : polygons_) {
+    if (!polygon->getEnabled()) {
+      continue;
+    }
     if (robot_action.action_type == STOP) {
       // If robot already should stop, do nothing
       break;
@@ -481,7 +486,9 @@ void CollisionMonitor::printAction(
 void CollisionMonitor::publishPolygons() const
 {
   for (std::shared_ptr<Polygon> polygon : polygons_) {
-    polygon->publish();
+    if (polygon->getEnabled()) {
+      polygon->publish();
+    }
   }
 }
 

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"};

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_;
@@ -244,6 +254,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();
@@ -350,6 +364,26 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp
   return true;
 }
 
+rcl_interfaces::msg::SetParametersResult
+Polygon::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> 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."

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"};

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"};

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<rclcpp::Parameter> 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