nav2_collision_monitor dynamic parameters polygon and source enabled …

…for Humble (ros-navigation#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 <[email protected]> * Start backport action_state_ declaration in collision_monitor_node_test.cpp Signed-off-by: EnzoGhisoni <[email protected]> --------- Signed-off-by: EnzoGhisoni <[email protected]> Co-authored-by: EnzoGhisoni <[email protected]>
EnjoyRobotics · Sep 26, 2024 · 16ae071 · 16ae071
1 parent bfd19ae
commit 16ae071
Show file tree

Hide file tree

Showing 12 changed files with 144 additions and 3 deletions.
diff --git 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
@@ -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

diff --git 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<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

diff --git 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
@@ -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> 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<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;
@@ -482,7 +487,9 @@ void CollisionMonitor::printAction(
 void CollisionMonitor::publishPolygons() const
 {
   for (std::shared_ptr<Polygon> 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
@@ -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
@@ -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<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."

diff --git 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
@@ -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
@@ -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
diff --git 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
@@ -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