collision_monitor: dynamic polygon and source enable/disable (ros-nav…

…igation#3825)

* Rename PushRosNamespace to PushROSNamespace

* Fix min_points checking

* initial

* fix

* fix

* remove unrelated change

* reset

* fix format

* PR fixes

* Add test

* fix comments

* add to params

* publish only if enabled

* Add source dynamic enable/disable

* add enabled param to sources

* fix

* add same to collision detector

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

@@ -85,6 +85,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 minimum points to enter inside polygon causing the action
    * @return Minimum number of data readings within a zone to trigger the action
@@ -191,6 +196,13 @@ class Polygon
    */
   void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg);
 
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
+    std::vector<rclcpp::Parameter> parameters);
+
   /**
    * @brief Checks if point is inside polygon
    * @param point Given point to check
@@ -204,6 +216,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
@@ -222,6 +236,8 @@ class Polygon
   double time_before_collision_;
   /// @brief Time step for robot movement simulation
   double simulation_time_step_;
+  /// @brief Whether polygon is enabled
+  bool enabled_;
   /// @brief Polygon subscription
   rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_sub_;
   /// @brief Footprint subscriber

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_detector_params.yaml

@@ -14,12 +14,15 @@ collision_detector:
       min_points: 4
       visualize: True
       polygon_pub_topic: "polygon_front"
+      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/params/collision_monitor_params.yaml

@@ -22,6 +22,7 @@ collision_monitor:
       min_points: 4
       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
     PolygonLimit:
       type: "polygon"
       points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5]
@@ -39,6 +41,7 @@ collision_monitor:
       angular_limit: 0.5
       visualize: True
       polygon_pub_topic: "polygon_limit"
+      enabled: True
     FootprintApproach:
       type: "polygon"
       action_type: "approach"
@@ -47,12 +50,15 @@ collision_monitor:
       simulation_time_step: 0.1
       min_points: 6
       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_detector_node.cpp

@@ -304,13 +304,18 @@ void CollisionDetector::process()
 
   // 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);
+    }
   }
 
   std::unique_ptr<nav2_msgs::msg::CollisionDetectorState> state_msg =
     std::make_unique<nav2_msgs::msg::CollisionDetectorState>();
 
   for (std::shared_ptr<Polygon> polygon : polygons_) {
+    if (!polygon->getEnabled()) {
+      continue;
+    }
     state_msg->polygons.push_back(polygon->getName());
     state_msg->detections.push_back(
       polygon->getPointsInside(
@@ -326,7 +331,9 @@ void CollisionDetector::process()
 void CollisionDetector::publishPolygons() const
 {
   for (std::shared_ptr<Polygon> polygon : polygons_) {
-    polygon->publish();
+    if (polygon->getEnabled()) {
+      polygon->publish();
+    }
   }
 }
 

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -373,7 +373,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
@@ -382,6 +384,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;
@@ -545,7 +550,9 @@ void CollisionMonitor::notifyActionState(
 void CollisionMonitor::publishPolygons() const
 {
   for (std::shared_ptr<Polygon> polygon : polygons_) {
-    polygon->publish();
+    if (polygon->getEnabled()) {
+      polygon->publish();
+    }
   }
 }
 

nav2_collision_monitor/src/pointcloud.cpp

@@ -49,6 +49,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

@@ -48,6 +48,7 @@ Polygon::~Polygon()
   polygon_sub_.reset();
   polygon_pub_.reset();
   poly_.clear();
+  dyn_params_handler_.reset();
 }
 
 bool Polygon::configure()
@@ -102,6 +103,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;
 }
 
@@ -129,6 +134,11 @@ ActionType Polygon::getActionType() const
   return action_type_;
 }
 
+bool Polygon::getEnabled() const
+{
+  return enabled_;
+}
+
 int Polygon::getMinPoints() const
 {
   return min_points_;
@@ -302,6 +312,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_ + ".min_points", rclcpp::ParameterValue(4));
     min_points_ = node->get_parameter(polygon_name_ + ".min_points").as_int();
@@ -487,6 +501,26 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m
   polygon_ = *msg;
 }
 
+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;
+}
+
 void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)
 {
   RCLCPP_INFO(

nav2_collision_monitor/src/range.cpp

@@ -49,6 +49,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

@@ -47,6 +47,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

@@ -43,6 +43,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();
@@ -54,6 +65,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(
@@ -75,4 +90,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