Merge branch 'add-collision-points-debug' into add-polygon-source

Author: Tony Najjar

nav2_collision_monitor/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(nav2_common REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 
 ### Header ###
 
@@ -37,6 +38,7 @@ set(dependencies
   nav2_util
   nav2_costmap_2d
   nav2_msgs
+  visualization_msgs
 )
 
 set(monitor_executable_name collision_monitor)

nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp

@@ -28,6 +28,7 @@
 
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/msg/collision_detector_state.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/polygon.hpp"
@@ -147,11 +148,16 @@ class CollisionDetector : public nav2_util::LifecycleNode
   /// @brief collision monitor state publisher
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionDetectorState>::SharedPtr
     state_pub_;
+  /// @brief Collision points marker publisher
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    collision_points_marker_pub_;
   /// @brief timer that runs actions
   rclcpp::TimerBase::SharedPtr timer_;
 
   /// @brief main loop frequency
   double frequency_;
+  /// @brief whether to publish collision points
+  bool visualize_collision_points_;
 };  // class CollisionDetector
 
 }  // namespace nav2_collision_monitor

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -21,6 +21,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
@@ -108,12 +109,15 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    * @param cmd_vel_in_topic Output name of cmd_vel_in topic
    * @param cmd_vel_out_topic Output name of cmd_vel_out topic
    * is required.
+   * @param state_topic topic name for publishing collision monitor state
+   * @param visualize_collision_points boolean flag to enable/disable collision points visualization
    * @return True if all parameters were obtained or false in failure case
    */
   bool getParameters(
     std::string & cmd_vel_in_topic,
     std::string & cmd_vel_out_topic,
-    std::string & state_topic);
+    std::string & state_topic,
+    bool & visualize_collision_points);
   /**
    * @brief Supporting routine creating and configuring all polygons
    * @param base_frame_id Robot base frame ID
@@ -211,6 +215,10 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionMonitorState>::SharedPtr
     state_pub_;
 
+  /// @brief Collision points marker publisher
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    collision_points_marker_pub_;
+
   /// @brief Whether main routine is active
   bool process_active_;
 

nav2_collision_monitor/package.xml

@@ -21,6 +21,7 @@
   <depend>nav2_util</depend>
   <depend>nav2_costmap_2d</depend>
   <depend>nav2_msgs</depend>
+  <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>

nav2_collision_monitor/params/collision_detector_params.yaml

@@ -6,6 +6,7 @@ collision_detector:
     transform_tolerance: 0.5
     source_timeout: 5.0
     base_shift_correction: True
+    visualize_collision_points: False
     polygons: ["PolygonFront"]
     PolygonFront:
       type: "polygon"

nav2_collision_monitor/params/collision_monitor_params.yaml

@@ -8,6 +8,7 @@ collision_monitor:
     transform_tolerance: 0.5
     source_timeout: 5.0
     base_shift_correction: True
+    visualize_collision_points: False
     stop_pub_timeout: 2.0
     # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
     # and robot footprint for "approach" action type.

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -55,6 +55,11 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/)
   state_pub_ = this->create_publisher<nav2_msgs::msg::CollisionDetectorState>(
     "collision_detector_state", rclcpp::SystemDefaultsQoS());
 
+  if (visualize_collision_points_) {
+    collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "collision_points_marker", 1);
+  }
+
   // Obtaining ROS parameters
   if (!getParameters()) {
     return nav2_util::CallbackReturn::FAILURE;
@@ -70,6 +75,9 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/)
 
   // Activating lifecycle publisher
   state_pub_->on_activate();
+  if (collision_points_marker_pub_) {
+    collision_points_marker_pub_->on_activate();
+  }
 
   // Activating polygons
   for (std::shared_ptr<Polygon> polygon : polygons_) {
@@ -97,6 +105,9 @@ CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 
   // Deactivating lifecycle publishers
   state_pub_->on_deactivate();
+  if (collision_points_marker_pub_) {
+    collision_points_marker_pub_->on_deactivate();
+  }
 
   // Deactivating polygons
   for (std::shared_ptr<Polygon> polygon : polygons_) {
@@ -115,6 +126,7 @@ CollisionDetector::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   RCLCPP_INFO(get_logger(), "Cleaning up");
 
   state_pub_.reset();
+  collision_points_marker_pub_.reset();
 
   polygons_.clear();
   sources_.clear();
@@ -143,6 +155,9 @@ bool CollisionDetector::getParameters()
   nav2_util::declare_parameter_if_not_declared(
     node, "frequency", rclcpp::ParameterValue(10.0));
   frequency_ = get_parameter("frequency").as_double();
+  nav2_util::declare_parameter_if_not_declared(
+    node, "visualize_collision_points", rclcpp::ParameterValue(false));
+  visualize_collision_points_ = get_parameter("visualize_collision_points").as_bool();
   nav2_util::declare_parameter_if_not_declared(
     node, "base_frame_id", rclcpp::ParameterValue("base_footprint"));
   base_frame_id = get_parameter("base_frame_id").as_string();
@@ -309,6 +324,33 @@ void CollisionDetector::process()
     }
   }
 
+  if (collision_points_marker_pub_) {
+    // visualize collision points with markers
+    visualization_msgs::msg::MarkerArray marker_array;
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = get_parameter("base_frame_id").as_string();
+    marker.header.stamp = rclcpp::Time(0, 0);
+    marker.ns = "collision_points";
+    marker.id = 0;
+    marker.type = visualization_msgs::msg::Marker::POINTS;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    marker.scale.x = 0.02;
+    marker.scale.y = 0.02;
+    marker.color.r = 1.0;
+    marker.color.a = 1.0;
+    marker.lifetime = rclcpp::Duration(0, 0);
+
+    for (const auto & point : collision_points) {
+      geometry_msgs::msg::Point p;
+      p.x = point.x;
+      p.y = point.y;
+      p.z = 0.0;
+      marker.points.push_back(p);
+    }
+    marker_array.markers.push_back(marker);
+    collision_points_marker_pub_->publish(marker_array);
+  }
+
   std::unique_ptr<nav2_msgs::msg::CollisionDetectorState> state_msg =
     std::make_unique<nav2_msgs::msg::CollisionDetectorState>();
 

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -57,9 +57,13 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
   std::string cmd_vel_in_topic;
   std::string cmd_vel_out_topic;
   std::string state_topic;
+  bool visualize_collision_points;
 
   // Obtaining ROS parameters
-  if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) {
+  if (!getParameters(
+      cmd_vel_in_topic, cmd_vel_out_topic, state_topic,
+      visualize_collision_points))
+  {
     return nav2_util::CallbackReturn::FAILURE;
   }
 
@@ -68,11 +72,16 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
     std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1));
   cmd_vel_out_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
     cmd_vel_out_topic, 1);
+
   if (!state_topic.empty()) {
     state_pub_ = this->create_publisher<nav2_msgs::msg::CollisionMonitorState>(
       state_topic, 1);
   }
 
+  if (visualize_collision_points) {
+    collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "collision_points_marker", 1);
+  }
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -86,6 +95,9 @@ CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/)
   if (state_pub_) {
     state_pub_->on_activate();
   }
+  if (collision_points_marker_pub_) {
+    collision_points_marker_pub_->on_activate();
+  }
 
   // Activating polygons
   for (std::shared_ptr<Polygon> polygon : polygons_) {
@@ -126,7 +138,9 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   if (state_pub_) {
     state_pub_->on_deactivate();
   }
-
+  if (collision_points_marker_pub_) {
+    collision_points_marker_pub_->on_deactivate();
+  }
   // Destroying bond connection
   destroyBond();
 
@@ -141,6 +155,7 @@ CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   cmd_vel_in_sub_.reset();
   cmd_vel_out_pub_.reset();
   state_pub_.reset();
+  collision_points_marker_pub_.reset();
 
   polygons_.clear();
   sources_.clear();
@@ -196,7 +211,8 @@ void CollisionMonitor::publishVelocity(const Action & robot_action)
 bool CollisionMonitor::getParameters(
   std::string & cmd_vel_in_topic,
   std::string & cmd_vel_out_topic,
-  std::string & state_topic)
+  std::string & state_topic,
+  bool & visualize_collision_points)
 {
   std::string base_frame_id, odom_frame_id;
   tf2::Duration transform_tolerance;
@@ -213,6 +229,9 @@ bool CollisionMonitor::getParameters(
   nav2_util::declare_parameter_if_not_declared(
     node, "state_topic", rclcpp::ParameterValue(""));
   state_topic = get_parameter("state_topic").as_string();
+  nav2_util::declare_parameter_if_not_declared(
+    node, "visualize_collision_points", rclcpp::ParameterValue(false));
+  visualize_collision_points = get_parameter("visualize_collision_points").as_bool();
 
   nav2_util::declare_parameter_if_not_declared(
     node, "base_frame_id", rclcpp::ParameterValue("base_footprint"));
@@ -385,6 +404,33 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
     }
   }
 
+  if (collision_points_marker_pub_) {
+    // visualize collision points with markers
+    visualization_msgs::msg::MarkerArray marker_array;
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = get_parameter("base_frame_id").as_string();
+    marker.header.stamp = rclcpp::Time(0, 0);
+    marker.ns = "collision_points";
+    marker.id = 0;
+    marker.type = visualization_msgs::msg::Marker::POINTS;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    marker.scale.x = 0.02;
+    marker.scale.y = 0.02;
+    marker.color.r = 1.0;
+    marker.color.a = 1.0;
+    marker.lifetime = rclcpp::Duration(0, 0);
+
+    for (const auto & point : collision_points) {
+      geometry_msgs::msg::Point p;
+      p.x = point.x;
+      p.y = point.y;
+      p.z = 0.0;
+      marker.points.push_back(p);
+    }
+    marker_array.markers.push_back(marker);
+    collision_points_marker_pub_->publish(marker_array);
+  }
+
   // By default - there is no action
   Action robot_action{DO_NOTHING, cmd_vel_in, ""};
   // Polygon causing robot action (if any)