diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 51cafe71dd0..b902cc4ea83 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/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) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index c4c5906b22a..7791265179f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/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::SharedPtr state_pub_; + /// @brief Collision points marker publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + collision_points_marker_pub_; /// @brief timer that runs actions rclcpp::TimerBase::SharedPtr timer_; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 0383dee3634..09a13658092 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/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::SharedPtr state_pub_; + /// @brief Collision points marker publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + collision_points_marker_pub_; + /// @brief Whether main routine is active bool process_active_; diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index a63c6b2fd59..4889ea77f16 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -21,6 +21,7 @@ nav2_util nav2_costmap_2d nav2_msgs + visualization_msgs ament_cmake_gtest ament_lint_common diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 6a5aca7dfec..d5cf99a9de7 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/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( "collision_detector_state", rclcpp::SystemDefaultsQoS()); + collision_points_marker_pub_ = this->create_publisher( + "~/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 : 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 : 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::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 state_msg = std::make_unique(); diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index a1395a7b70e..fd1163ed32f 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/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( cmd_vel_out_topic, 1); + if (!state_topic.empty()) { state_pub_ = this->create_publisher( state_topic, 1); } + collision_points_marker_pub_ = this->create_publisher( + "~/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 : 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::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) diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index eab3e177548..b0b926fde05 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/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::SharedPtr state_sub_; nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_; + + // CollisionMonitor collision points markers + rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; + }; // Tester Tester::Tester() @@ -172,6 +181,10 @@ Tester::Tester() state_sub_ = cd_->create_subscription( STATE_TOPIC, rclcpp::SystemDefaultsQoS(), std::bind(&Tester::stateCallback, this, std::placeholders::_1)); + + collision_points_marker_sub_ = cd_->create_subscription( + COLLISION_POINTS_MARKERS_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::collisionPointsMarkerCallback, this, std::placeholders::_1)); } Tester::~Tester() @@ -179,6 +192,7 @@ 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 diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index e66e33e5cb4..316bd0c1d06 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_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" @@ -48,6 +49,7 @@ static const char ODOM_FRAME_ID[]{"odom"}; static const char CMD_VEL_IN_TOPIC[]{"cmd_vel_in"}; static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"}; static const char STATE_TOPIC[]{"collision_monitor_state"}; +static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_monitor/collision_points_marker"}; static const char FOOTPRINT_TOPIC[]{"footprint"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; @@ -164,10 +166,12 @@ class Tester : public ::testing::Test rclcpp::Client::SharedFuture, const std::chrono::nanoseconds & timeout); bool waitActionState(const std::chrono::nanoseconds & timeout); + bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout); protected: void cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg); void actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPtr msg); + void collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg); // CollisionMonitor node std::shared_ptr cm_; @@ -190,6 +194,10 @@ class Tester : public ::testing::Test rclcpp::Subscription::SharedPtr action_state_sub_; nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; + // CollisionMonitor collision points markers + rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; + // Service client for setting CollisionMonitor parameters rclcpp::Client::SharedPtr parameters_client_; }; // Tester @@ -217,6 +225,9 @@ Tester::Tester() action_state_sub_ = cm_->create_subscription( STATE_TOPIC, rclcpp::SystemDefaultsQoS(), std::bind(&Tester::actionStateCallback, this, std::placeholders::_1)); + collision_points_marker_sub_ = cm_->create_subscription( + COLLISION_POINTS_MARKERS_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::collisionPointsMarkerCallback, this, std::placeholders::_1)); parameters_client_ = cm_->create_client( std::string( @@ -235,6 +246,7 @@ Tester::~Tester() cmd_vel_out_sub_.reset(); action_state_sub_.reset(); + collision_points_marker_sub_.reset(); cm_.reset(); } @@ -554,6 +566,7 @@ void Tester::publishCmdVel(const double x, const double y, const double tw) // Reset cmd_vel_out_ before calling CollisionMonitor::process() cmd_vel_out_ = nullptr; action_state_ = nullptr; + collision_points_marker_msg_ = nullptr; std::unique_ptr msg = std::make_unique(); @@ -623,6 +636,19 @@ bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + if (collision_points_marker_msg_) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + void Tester::cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg) { cmd_vel_out_ = msg; @@ -633,6 +659,11 @@ void Tester::actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPt action_state_ = msg; } +void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg) +{ + collision_points_marker_msg_ = msg; +} + TEST_F(Tester, testProcessStopSlowdownLimit) { rclcpp::Time curr_time = cm_->now(); @@ -1170,6 +1201,35 @@ TEST_F(Tester, testSourcesNotSet) cm_->cant_configure(); } +TEST_F(Tester, testCollisionPointsMarkers) +{ + rclcpp::Time curr_time = cm_->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 + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); + + publishCmdVel(0.5, 0.2, 0.1); + 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 + cm_->stop(); +} + int main(int argc, char ** argv) { // Initialize the system