From 5f8281e581348ff6978cfb53727483a84c008819 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 17 Aug 2023 14:20:51 +0200 Subject: [PATCH 01/11] Rename PushRosNamespace to PushROSNamespace --- nav2_bringup/launch/bringup_launch.py | 4 ++-- .../launch/collision_detector_node.launch.py | 6 +++--- .../launch/collision_monitor_node.launch.py | 6 +++--- nav2_system_tests/src/system/test_multi_robot_launch.py | 4 ++-- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 24668627c8..8af81ac28e 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -23,7 +23,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace +from launch_ros.actions import PushROSNamespace from launch_ros.descriptions import ParameterFile from nav2_common.launch import RewrittenYaml @@ -113,7 +113,7 @@ def generate_launch_description(): # Specify the actions bringup_cmd_group = GroupAction([ - PushRosNamespace( + PushROSNamespace( condition=IfCondition(use_namespace), namespace=namespace), diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py index ccd58c2e15..5440cfedc3 100644 --- a/nav2_collision_monitor/launch/collision_detector_node.launch.py +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -25,7 +25,7 @@ from launch.substitutions import NotEqualsSubstitution from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace +from launch_ros.actions import PushROSNamespace from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml @@ -84,7 +84,7 @@ def generate_launch_description(): condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ SetParameter('use_sim_time', use_sim_time), - PushRosNamespace( + PushROSNamespace( condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), namespace=namespace), Node( @@ -110,7 +110,7 @@ def generate_launch_description(): condition=IfCondition(use_composition), actions=[ SetParameter('use_sim_time', use_sim_time), - PushRosNamespace( + PushROSNamespace( condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), namespace=namespace), LoadComposableNodes( diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index 3d0006ac85..f4d0a953e3 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -25,7 +25,7 @@ from launch.substitutions import NotEqualsSubstitution from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace +from launch_ros.actions import PushROSNamespace from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -86,7 +86,7 @@ def generate_launch_description(): condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ SetParameter('use_sim_time', use_sim_time), - PushRosNamespace( + PushROSNamespace( condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), namespace=namespace), Node( @@ -112,7 +112,7 @@ def generate_launch_description(): condition=IfCondition(use_composition), actions=[ SetParameter('use_sim_time', use_sim_time), - PushRosNamespace( + PushROSNamespace( condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), namespace=namespace), LoadComposableNodes( diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index d323f97567..ea7e00db93 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -24,7 +24,7 @@ IncludeLaunchDescription, SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import TextSubstitution -from launch_ros.actions import Node, PushRosNamespace +from launch_ros.actions import Node, PushROSNamespace from launch_testing.legacy import LaunchTestService @@ -95,7 +95,7 @@ def generate_launch_description(): group = GroupAction([ # Instances use the robot's name for namespace - PushRosNamespace(robot['name']), + PushROSNamespace(robot['name']), IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), From 212bbbd497edf8867d77cdaae0bf0eae34d4ae88 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Sep 2023 14:52:59 +0200 Subject: [PATCH 02/11] Fix min_points checking --- nav2_collision_monitor/src/collision_detector_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index e12d9795db..98433ead04 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -314,7 +314,7 @@ void CollisionDetector::process() state_msg->polygons.push_back(polygon->getName()); state_msg->detections.push_back( polygon->getPointsInside( - collision_points) > polygon->getMinPoints()); + collision_points) >= polygon->getMinPoints()); } state_pub_->publish(std::move(state_msg)); From 183e077d397b2a8321dfe299ca9d8f2fae9c13a0 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 13 Oct 2023 17:11:11 +0200 Subject: [PATCH 03/11] . --- nav2_collision_monitor/CMakeLists.txt | 2 + .../collision_monitor_node.hpp | 10 +++- nav2_collision_monitor/package.xml | 1 + .../src/collision_monitor_node.cpp | 55 ++++++++++++++++++- 4 files changed, 64 insertions(+), 4 deletions(-) diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 51cafe71dd..b902cc4ea8 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_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 0383dee363..ed9bd74422 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,12 +108,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 @@ -210,6 +214,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 a63c6b2fd5..4889ea77f1 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_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 8b7984a9c7..4ef94d78e2 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/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( cmd_vel_out_topic, 1); + if (!state_topic.empty()) { state_pub_ = this->create_publisher( state_topic, 1); } + if (visualize_collision_points) { + collision_points_marker_pub_ = this->create_publisher( + "~/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 : 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")); @@ -376,6 +395,36 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) source->getData(curr_time, collision_points); } + if (collision_points_marker_pub_) { + // visualize collision points with markers + visualization_msgs::msg::MarkerArray marker_array; + if (collision_points.size() > 0) { + 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.1; + marker.scale.y = 0.1; + marker.color.r = 1.0; + marker.color.a = 1.0; + marker.lifetime = rclcpp::Duration(2, 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) From 86145e3aa246853d24d7bd88d4a3c9b8bd93d185 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 16 Oct 2023 10:23:07 +0200 Subject: [PATCH 04/11] fixes --- nav2_collision_monitor/src/collision_monitor_node.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 912a5b3f39..d15b6f1084 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -80,7 +80,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) if (visualize_collision_points) { collision_points_marker_pub_ = this->create_publisher( - "~/collision_points_marker", 1); + "collision_points_marker", 1); } return nav2_util::CallbackReturn::SUCCESS; } @@ -408,11 +408,11 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) marker.id = 0; marker.type = visualization_msgs::msg::Marker::POINTS; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale.x = 0.1; - marker.scale.y = 0.1; + marker.scale.x = 0.02; + marker.scale.y = 0.02; marker.color.r = 1.0; marker.color.a = 1.0; - marker.lifetime = rclcpp::Duration(2, 0); + marker.lifetime = rclcpp::Duration(0, 0); for (const auto & point : collision_points) { geometry_msgs::msg::Point p; @@ -426,7 +426,6 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) 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) From cce70c8bb6a266971151e774bfed5812810df638 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 16 Oct 2023 11:01:03 +0200 Subject: [PATCH 05/11] add to collision detector --- .../collision_detector_node.hpp | 6 +++ .../params/collision_detector_params.yaml | 1 + .../params/collision_monitor_params.yaml | 1 + .../src/collision_detector_node.cpp | 42 +++++++++++++++++++ .../src/collision_monitor_node.cpp | 42 +++++++++---------- 5 files changed, 70 insertions(+), 22 deletions(-) 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 c4c5906b22..a8f6f3f727 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,11 +148,16 @@ 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_; /// @brief main loop frequency double frequency_; + /// @brief whether to publish collision points + bool visualize_collision_points_; }; // class CollisionDetector } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index 68b4cd7b50..d77956fc4c 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/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" diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index fcb01f5482..9b219ff5a4 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/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. diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 6a5aca7dfe..c7af3c377b 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/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( "collision_detector_state", rclcpp::SystemDefaultsQoS()); + if (visualize_collision_points_) { + collision_points_marker_pub_ = this->create_publisher( + "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 : 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 : 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(); @@ -308,6 +323,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 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 d15b6f1084..daceadda26 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -400,29 +400,27 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) if (collision_points_marker_pub_) { // visualize collision points with markers visualization_msgs::msg::MarkerArray marker_array; - if (collision_points.size() > 0) { - 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); + 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); } From 8abb418eb7d966732880f33fecd6de3372b71317 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 17 Oct 2023 12:52:50 +0200 Subject: [PATCH 06/11] fix --- .../collision_detector_node.hpp | 2 -- .../collision_monitor_node.hpp | 4 +-- .../params/collision_detector_params.yaml | 1 - .../params/collision_monitor_params.yaml | 1 - .../src/collision_detector_node.cpp | 25 +++++--------- .../src/collision_monitor_node.cpp | 34 +++++++------------ 6 files changed, 21 insertions(+), 46 deletions(-) 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 a8f6f3f727..7791265179 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 @@ -156,8 +156,6 @@ class CollisionDetector : public nav2_util::LifecycleNode /// @brief main loop frequency double frequency_; - /// @brief whether to publish collision points - bool visualize_collision_points_; }; // class CollisionDetector } // namespace nav2_collision_monitor 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 ed9bd74422..09a1365809 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 @@ -109,14 +109,12 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @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, - bool & visualize_collision_points); + std::string & state_topic); /** * @brief Supporting routine creating and configuring all polygons * @param base_frame_id Robot base frame ID diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index d77956fc4c..68b4cd7b50 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -6,7 +6,6 @@ collision_detector: transform_tolerance: 0.5 source_timeout: 5.0 base_shift_correction: True - visualize_collision_points: False polygons: ["PolygonFront"] PolygonFront: type: "polygon" diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 9b219ff5a4..fcb01f5482 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -8,7 +8,6 @@ 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. diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index c7af3c377b..26a33c792a 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -55,10 +55,8 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) state_pub_ = this->create_publisher( "collision_detector_state", rclcpp::SystemDefaultsQoS()); - if (visualize_collision_points_) { - collision_points_marker_pub_ = this->create_publisher( - "collision_points_marker", 1); - } + collision_points_marker_pub_ = this->create_publisher( + "collision_points_marker", 1); // Obtaining ROS parameters if (!getParameters()) { @@ -75,9 +73,7 @@ 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(); - } + collision_points_marker_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -105,9 +101,7 @@ 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(); - } + collision_points_marker_pub_->on_deactivate(); // Deactivating polygons for (std::shared_ptr polygon : polygons_) { @@ -155,9 +149,6 @@ 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(); @@ -323,9 +314,9 @@ void CollisionDetector::process() } } - if (collision_points_marker_pub_) { + if (collision_points_marker_pub_->get_subscription_count() > 0) { // visualize collision points with markers - visualization_msgs::msg::MarkerArray marker_array; + 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); @@ -346,8 +337,8 @@ void CollisionDetector::process() p.z = 0.0; marker.points.push_back(p); } - marker_array.markers.push_back(marker); - collision_points_marker_pub_->publish(marker_array); + marker_array->markers.push_back(marker); + collision_points_marker_pub_->publish(std::move(marker_array)); } std::unique_ptr state_msg = diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index daceadda26..3a007e7faa 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -57,12 +57,10 @@ 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, - visualize_collision_points)) + cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) { return nav2_util::CallbackReturn::FAILURE; } @@ -78,10 +76,9 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) state_topic, 1); } - if (visualize_collision_points) { - collision_points_marker_pub_ = this->create_publisher( - "collision_points_marker", 1); - } + collision_points_marker_pub_ = this->create_publisher( + "collision_points_marker", 1); + return nav2_util::CallbackReturn::SUCCESS; } @@ -95,9 +92,7 @@ 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(); - } + collision_points_marker_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -138,9 +133,8 @@ 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(); - } + collision_points_marker_pub_->on_deactivate(); + // Destroying bond connection destroyBond(); @@ -211,8 +205,7 @@ 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, - bool & visualize_collision_points) + std::string & state_topic) { std::string base_frame_id, odom_frame_id; tf2::Duration transform_tolerance; @@ -229,9 +222,6 @@ 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")); @@ -397,9 +387,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) } } - if (collision_points_marker_pub_) { + if (collision_points_marker_pub_->get_subscription_count() > 0) { // visualize collision points with markers - visualization_msgs::msg::MarkerArray marker_array; + 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); @@ -420,8 +410,8 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) p.z = 0.0; marker.points.push_back(p); } - marker_array.markers.push_back(marker); - collision_points_marker_pub_->publish(marker_array); + marker_array->markers.push_back(marker); + collision_points_marker_pub_->publish(std::move(marker_array)); } // By default - there is no action From ff07238f3eb57542e1837bb49842be683c6bb2fd Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 17 Oct 2023 12:54:46 +0200 Subject: [PATCH 07/11] fix --- nav2_collision_monitor/src/collision_monitor_node.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 3a007e7faa..a4fb3bd3ae 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -59,9 +59,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) std::string state_topic; // 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)) { return nav2_util::CallbackReturn::FAILURE; } From 9df3c88f281ffe544020f1c57c6be4b8078490b9 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 17 Oct 2023 14:39:37 +0200 Subject: [PATCH 08/11] . --- .../test/collision_monitor_node_test.cpp | 66 +++++++++++++++++++ 1 file changed, 66 insertions(+) diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index e66e33e5cb..bc94a5fe07 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_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 Action state + 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(); } @@ -623,6 +635,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 +658,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 +1200,42 @@ 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(); + addPolygon("Limit", POLYGON, 3.0, "limit"); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + addSource(POINTCLOUD_NAME, POINTCLOUD); + addSource(RANGE_NAME, RANGE); + setVectors({"Limit", "SlowDown", "Stop"}, {SCAN_NAME, POINTCLOUD_NAME, RANGE_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is far away from robot + 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 From a976020a6ebc47fe4a2edc3c38ed7f01472b9f04 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 18 Oct 2023 14:51:23 +0200 Subject: [PATCH 09/11] fixes --- .../src/collision_detector_node.cpp | 1 + .../test/collision_detector_node_test.cpp | 60 +++++++++++++++++++ .../test/collision_monitor_node_test.cpp | 12 +--- 3 files changed, 64 insertions(+), 9 deletions(-) diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 26a33c792a..7ea197556c 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -331,6 +331,7 @@ void CollisionDetector::process() marker.lifetime = rclcpp::Duration(0, 0); for (const auto & point : collision_points) { + std::cout << "collision point: " << point.x << ", " << point.y << std::endl; geometry_msgs::msg::Point p; p.x = point.x; p.y = point.y; diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index eab3e17754..28ee7130c8 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_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 bc94a5fe07..7f67c21089 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -194,7 +194,7 @@ class Tester : public ::testing::Test rclcpp::Subscription::SharedPtr action_state_sub_; nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; - // CollisionMonitor Action state + // CollisionMonitor collision points markers rclcpp::Subscription::SharedPtr collision_points_marker_sub_; visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; @@ -566,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(); @@ -1207,13 +1208,8 @@ TEST_F(Tester, testCollisionPointsMarkers) // Set Collision Monitor parameters. // Making two polygons: outer polygon for slowdown and inner for robot stop. setCommonParameters(); - addPolygon("Limit", POLYGON, 3.0, "limit"); - addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); - addPolygon("Stop", POLYGON, 1.0, "stop"); addSource(SCAN_NAME, SCAN); - addSource(POINTCLOUD_NAME, POINTCLOUD); - addSource(RANGE_NAME, RANGE); - setVectors({"Limit", "SlowDown", "Stop"}, {SCAN_NAME, POINTCLOUD_NAME, RANGE_NAME}); + setVectors({}, {SCAN_NAME}); // Start Collision Monitor node cm_->start(); @@ -1221,7 +1217,6 @@ TEST_F(Tester, testCollisionPointsMarkers) // Share TF sendTransforms(curr_time); - // 1. Obstacle is far away from robot publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCollisionPointsMarker(500ms)); ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); @@ -1229,7 +1224,6 @@ TEST_F(Tester, testCollisionPointsMarkers) 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 From 44c8a06bad78242639d551f23c7199eb720fca22 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 18 Oct 2023 17:40:18 +0200 Subject: [PATCH 10/11] add namespace to topic --- nav2_collision_monitor/src/collision_detector_node.cpp | 2 +- nav2_collision_monitor/src/collision_monitor_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 7ea197556c..b04a642e06 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -56,7 +56,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) "collision_detector_state", rclcpp::SystemDefaultsQoS()); collision_points_marker_pub_ = this->create_publisher( - "collision_points_marker", 1); + "~/collision_points_marker", 1); // Obtaining ROS parameters if (!getParameters()) { diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index a4fb3bd3ae..fd1163ed32 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -75,7 +75,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) } collision_points_marker_pub_ = this->create_publisher( - "collision_points_marker", 1); + "~/collision_points_marker", 1); return nav2_util::CallbackReturn::SUCCESS; } From c7380b3dd57a218ff7b3094b207de8f9fcdcd91b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 19 Oct 2023 10:21:31 +0200 Subject: [PATCH 11/11] fixes --- nav2_collision_monitor/src/collision_detector_node.cpp | 1 - nav2_collision_monitor/test/collision_detector_node_test.cpp | 2 +- nav2_collision_monitor/test/collision_monitor_node_test.cpp | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index b04a642e06..d5cf99a9de 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -331,7 +331,6 @@ void CollisionDetector::process() marker.lifetime = rclcpp::Duration(0, 0); for (const auto & point : collision_points) { - std::cout << "collision point: " << point.x << ", " << point.y << std::endl; geometry_msgs::msg::Point p; p.x = point.x; p.y = point.y; diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 28ee7130c8..b0b926fde0 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -50,7 +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_points_marker"}; +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}; diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 7f67c21089..316bd0c1d0 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -49,7 +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_points_marker"}; +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"};