Publish collision points for debug purposes (ros-navigation#3879)

* Rename PushRosNamespace to PushROSNamespace

* Fix min_points checking

* .

* fixes

* add to collision detector

* fix

* fix

* .

* fixes

* add namespace to topic

* fixes

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,6 +148,9 @@ 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_;
 

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"
@@ -107,6 +108,7 @@ 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
    * @return True if all parameters were obtained or false in failure case
    */
   bool getParameters(
@@ -210,6 +212,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/src/collision_detector_node.cpp

@@ -55,6 +55,9 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/)
   state_pub_ = this->create_publisher<nav2_msgs::msg::CollisionDetectorState>(
     "collision_detector_state", rclcpp::SystemDefaultsQoS());
 
+  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 +73,7 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/)
 
   // Activating lifecycle publisher
   state_pub_->on_activate();
+  collision_points_marker_pub_->on_activate();
 
   // Activating polygons
   for (std::shared_ptr<Polygon> polygon : polygons_) {
@@ -97,6 +101,7 @@ CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 
   // Deactivating lifecycle publishers
   state_pub_->on_deactivate();
+  collision_points_marker_pub_->on_deactivate();
 
   // Deactivating polygons
   for (std::shared_ptr<Polygon> polygon : polygons_) {
@@ -115,6 +120,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();
@@ -308,6 +314,33 @@ void CollisionDetector::process()
     }
   }
 
+  if (collision_points_marker_pub_->get_subscription_count() > 0) {
+    // visualize collision points with markers
+    auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
+    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(std::move(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

@@ -68,11 +68,15 @@ 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);
   }
 
+  collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
+    "~/collision_points_marker", 1);
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -86,6 +90,7 @@ CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/)
   if (state_pub_) {
     state_pub_->on_activate();
   }
+  collision_points_marker_pub_->on_activate();
 
   // Activating polygons
   for (std::shared_ptr<Polygon> polygon : polygons_) {
@@ -126,6 +131,7 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   if (state_pub_) {
     state_pub_->on_deactivate();
   }
+  collision_points_marker_pub_->on_deactivate();
 
   // Destroying bond connection
   destroyBond();
@@ -141,6 +147,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();
@@ -378,6 +385,33 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
     }
   }
 
+  if (collision_points_marker_pub_->get_subscription_count() > 0) {
+    // visualize collision points with markers
+    auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
+    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(std::move(marker_array));
+  }
+
   // By default - there is no action
   Action robot_action{DO_NOTHING, cmd_vel_in, ""};
   // Polygon causing robot action (if any)

nav2_collision_monitor/test/collision_detector_node_test.cpp

@@ -32,6 +32,7 @@
 #include "sensor_msgs/point_cloud2_iterator.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/polygon_stamped.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #include "tf2_ros/transform_broadcaster.h"
 
@@ -49,6 +50,7 @@ static const char SCAN_NAME[]{"Scan"};
 static const char POINTCLOUD_NAME[]{"PointCloud"};
 static const char RANGE_NAME[]{"Range"};
 static const char STATE_TOPIC[]{"collision_detector_state"};
+static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_detector/collision_points_marker"};
 static const int MIN_POINTS{1};
 static const double SIMULATION_TIME_STEP{0.01};
 static const double TRANSFORM_TOLERANCE{0.5};
@@ -144,6 +146,8 @@ class Tester : public ::testing::Test
     const rclcpp::Time & stamp);
   bool waitState(const std::chrono::nanoseconds & timeout);
   void stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg);
+  bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout);
+  void collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg);
 
 protected:
   // CollisionDetector node
@@ -156,6 +160,11 @@ class Tester : public ::testing::Test
 
   rclcpp::Subscription<nav2_msgs::msg::CollisionDetectorState>::SharedPtr state_sub_;
   nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_;
+
+  // CollisionMonitor collision points markers
+  rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr collision_points_marker_sub_;
+  visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_;
+
 };  // Tester
 
 Tester::Tester()
@@ -172,13 +181,18 @@ Tester::Tester()
   state_sub_ = cd_->create_subscription<nav2_msgs::msg::CollisionDetectorState>(
     STATE_TOPIC, rclcpp::SystemDefaultsQoS(),
     std::bind(&Tester::stateCallback, this, std::placeholders::_1));
+
+  collision_points_marker_sub_ = cd_->create_subscription<visualization_msgs::msg::MarkerArray>(
+    COLLISION_POINTS_MARKERS_TOPIC, rclcpp::SystemDefaultsQoS(),
+    std::bind(&Tester::collisionPointsMarkerCallback, this, std::placeholders::_1));
 }
 
 Tester::~Tester()
 {
   scan_pub_.reset();
   pointcloud_pub_.reset();
   range_pub_.reset();
+  collision_points_marker_sub_.reset();
 
   cd_.reset();
 }
@@ -196,11 +210,30 @@ bool Tester::waitState(const std::chrono::nanoseconds & timeout)
   return false;
 }
 
+bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout)
+{
+  collision_points_marker_msg_ = nullptr;
+  rclcpp::Time start_time = cd_->now();
+  while (rclcpp::ok() && cd_->now() - start_time <= rclcpp::Duration(timeout)) {
+    if (collision_points_marker_msg_) {
+      return true;
+    }
+    rclcpp::spin_some(cd_->get_node_base_interface());
+    std::this_thread::sleep_for(10ms);
+  }
+  return false;
+}
+
 void Tester::stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg)
 {
   state_msg_ = msg;
 }
 
+void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg)
+{
+  collision_points_marker_msg_ = msg;
+}
+
 void Tester::setCommonParameters()
 {
   cd_->declare_parameter(
@@ -678,6 +711,33 @@ TEST_F(Tester, testPointcloudDetection)
   cd_->stop();
 }
 
+TEST_F(Tester, testCollisionPointsMarkers)
+{
+  rclcpp::Time curr_time = cd_->now();
+
+  // Set Collision Monitor parameters.
+  // Making two polygons: outer polygon for slowdown and inner for robot stop.
+  setCommonParameters();
+  addSource(SCAN_NAME, SCAN);
+  setVectors({}, {SCAN_NAME});
+
+  // Start Collision Monitor node
+  cd_->start();
+
+  // Share TF
+  sendTransforms(curr_time);
+
+  ASSERT_TRUE(waitCollisionPointsMarker(500ms));
+  ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u);
+
+  publishScan(0.5, curr_time);
+  ASSERT_TRUE(waitData(0.5, 500ms, curr_time));
+  ASSERT_TRUE(waitCollisionPointsMarker(500ms));
+  ASSERT_NE(collision_points_marker_msg_->markers[0].points.size(), 0u);
+  // Stop Collision Monitor node
+  cd_->stop();
+}
+
 int main(int argc, char ** argv)
 {
   // Initialize the system