From 7953bbfe5d141d64e25531935c4007298622c025 Mon Sep 17 00:00:00 2001 From: yenode Date: Sat, 14 Mar 2026 22:58:22 +0530 Subject: [PATCH 1/7] feat: publish collision string message during emergency stop (Resolves #159) Signed-off-by: Aditya Pachauri --- rmf_robot_sim_common/CMakeLists.txt | 3 +++ .../include/rmf_robot_sim_common/slotcar_common.hpp | 2 ++ rmf_robot_sim_common/package.xml | 1 + rmf_robot_sim_common/src/slotcar_common.cpp | 10 ++++++++++ 4 files changed, 16 insertions(+) diff --git a/rmf_robot_sim_common/CMakeLists.txt b/rmf_robot_sim_common/CMakeLists.txt index b02c513..b2a74d9 100644 --- a/rmf_robot_sim_common/CMakeLists.txt +++ b/rmf_robot_sim_common/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(rmf_fleet_msgs REQUIRED) @@ -74,6 +75,7 @@ target_link_libraries(slotcar_common Eigen3::Eigen rclcpp::rclcpp ${geometry_msgs_TARGETS} + ${std_msgs_TARGETS} ${rmf_fleet_msgs_TARGETS} ${rmf_building_map_msgs_TARGETS} tf2_ros::tf2_ros @@ -91,6 +93,7 @@ target_include_directories(slotcar_common ament_export_dependencies( Eigen3 rclcpp + std_msgs geometry_msgs tf2_ros rmf_fleet_msgs diff --git a/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp b/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp index 59977c4..76947c5 100644 --- a/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp +++ b/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -174,6 +175,7 @@ class SlotcarCommon std::shared_ptr _tf2_broadcaster; rclcpp::Publisher::SharedPtr _robot_state_pub; + rclcpp::Publisher::SharedPtr _collision_pub; rclcpp::Subscription::SharedPtr _traj_sub; rclcpp::Subscription::SharedPtr _pause_sub; diff --git a/rmf_robot_sim_common/package.xml b/rmf_robot_sim_common/package.xml index 21c3e11..2b31192 100644 --- a/rmf_robot_sim_common/package.xml +++ b/rmf_robot_sim_common/package.xml @@ -16,6 +16,7 @@ eigen rclcpp geometry_msgs + std_msgs tf2_ros rmf_fleet_msgs rmf_building_map_msgs diff --git a/rmf_robot_sim_common/src/slotcar_common.cpp b/rmf_robot_sim_common/src/slotcar_common.cpp index 487c25a..d8229c8 100644 --- a/rmf_robot_sim_common/src/slotcar_common.cpp +++ b/rmf_robot_sim_common/src/slotcar_common.cpp @@ -211,6 +211,10 @@ void SlotcarCommon::init_ros_node(const rclcpp::Node::SharedPtr node) _ros_node->create_publisher( "/robot_state", 10); + _collision_pub = + _ros_node->create_publisher( + "/robot_collisions", 10); + auto qos_profile = rclcpp::QoS(10); qos_profile.transient_local(); _building_map_sub = @@ -842,8 +846,14 @@ bool SlotcarCommon::emergency_stop( // TODO flush logger here // TODO get collision object name if (need_to_stop) + { RCLCPP_INFO_STREAM(logger(), "Stopping [" << _model_name << "] to avoid a collision"); + + std_msgs::msg::String msg; + msg.data = _model_name; + _collision_pub->publish(msg); + } else RCLCPP_INFO_STREAM(logger(), "No more obstacles; resuming course for [" << _model_name << "]"); From 998a808c4ee1cdd08bc81da5dd83d94eb13f1b04 Mon Sep 17 00:00:00 2001 From: Aditya Pachauri Date: Wed, 15 Apr 2026 01:38:56 +0530 Subject: [PATCH 2/7] feat: update emergency_stop to use RobotCollision message Signed-off-by: Aditya Pachauri --- .../rmf_robot_sim_common/slotcar_common.hpp | 4 ++-- rmf_robot_sim_common/src/slotcar_common.cpp | 19 +++++++++++++++---- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp b/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp index 76947c5..96e3ab7 100644 --- a/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp +++ b/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include #include @@ -175,7 +175,7 @@ class SlotcarCommon std::shared_ptr _tf2_broadcaster; rclcpp::Publisher::SharedPtr _robot_state_pub; - rclcpp::Publisher::SharedPtr _collision_pub; + rclcpp::Publisher::SharedPtr _collision_pub; rclcpp::Subscription::SharedPtr _traj_sub; rclcpp::Subscription::SharedPtr _pause_sub; diff --git a/rmf_robot_sim_common/src/slotcar_common.cpp b/rmf_robot_sim_common/src/slotcar_common.cpp index d8229c8..08aee5c 100644 --- a/rmf_robot_sim_common/src/slotcar_common.cpp +++ b/rmf_robot_sim_common/src/slotcar_common.cpp @@ -212,7 +212,7 @@ void SlotcarCommon::init_ros_node(const rclcpp::Node::SharedPtr node) "/robot_state", 10); _collision_pub = - _ros_node->create_publisher( + _ros_node->create_publisher( "/robot_collisions", 10); auto qos_profile = rclcpp::QoS(10); @@ -849,9 +849,20 @@ bool SlotcarCommon::emergency_stop( { RCLCPP_INFO_STREAM(logger(), "Stopping [" << _model_name << "] to avoid a collision"); - - std_msgs::msg::String msg; - msg.data = _model_name; + + rmf_fleet_msgs::msg::RobotCollision msg; + msg.robot_name = _model_name; + + msg.location.t = _ros_node->get_clock()->now(); + msg.location.x = _pose.translation()[0]; + msg.location.y = _pose.translation()[1]; + msg.location.yaw = compute_yaw(_pose); + msg.location.level_name = get_level_name(_pose.translation()[2]); + if (!_remaining_path.empty()) + { + msg.location.index = _remaining_path.front().index; + } + _collision_pub->publish(msg); } else From 2bf635a618b8b75a495fb550734d7d3e5307c442 Mon Sep 17 00:00:00 2001 From: Aditya Pachauri Date: Thu, 16 Apr 2026 12:28:27 +0530 Subject: [PATCH 3/7] trigger CI Signed-off-by: Aditya Pachauri From c924ead4d06204801df5087815c4997bf60cf40d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 4 May 2026 22:20:13 +0800 Subject: [PATCH 4/7] Remove unnecessary addition of std_msgs Signed-off-by: Michael X. Grey --- rmf_robot_sim_common/CMakeLists.txt | 3 --- rmf_robot_sim_common/package.xml | 1 - 2 files changed, 4 deletions(-) diff --git a/rmf_robot_sim_common/CMakeLists.txt b/rmf_robot_sim_common/CMakeLists.txt index b2a74d9..b02c513 100644 --- a/rmf_robot_sim_common/CMakeLists.txt +++ b/rmf_robot_sim_common/CMakeLists.txt @@ -20,7 +20,6 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(rmf_fleet_msgs REQUIRED) @@ -75,7 +74,6 @@ target_link_libraries(slotcar_common Eigen3::Eigen rclcpp::rclcpp ${geometry_msgs_TARGETS} - ${std_msgs_TARGETS} ${rmf_fleet_msgs_TARGETS} ${rmf_building_map_msgs_TARGETS} tf2_ros::tf2_ros @@ -93,7 +91,6 @@ target_include_directories(slotcar_common ament_export_dependencies( Eigen3 rclcpp - std_msgs geometry_msgs tf2_ros rmf_fleet_msgs diff --git a/rmf_robot_sim_common/package.xml b/rmf_robot_sim_common/package.xml index 2b31192..21c3e11 100644 --- a/rmf_robot_sim_common/package.xml +++ b/rmf_robot_sim_common/package.xml @@ -16,7 +16,6 @@ eigen rclcpp geometry_msgs - std_msgs tf2_ros rmf_fleet_msgs rmf_building_map_msgs From 1d62ca3fa4626150542686150e19bde6c6ef02c0 Mon Sep 17 00:00:00 2001 From: Aditya Pachauri Date: Fri, 29 May 2026 23:23:06 +0530 Subject: [PATCH 5/7] Trigger CI rebuild after network timeout Signed-off-by: Aditya Pachauri From 7ad5f314e4075dd0539355905ffaa55c36d78860 Mon Sep 17 00:00:00 2001 From: Aditya Pachauri Date: Fri, 29 May 2026 23:23:31 +0530 Subject: [PATCH 6/7] Trigger CI rebuild after network timeout Signed-off-by: Aditya Pachauri From bb53d99ff494cd680079f4a1779b9a6d809fb472 Mon Sep 17 00:00:00 2001 From: Aditya Pachauri Date: Fri, 29 May 2026 23:24:57 +0530 Subject: [PATCH 7/7] Trigger CI rebuild after network timeout Signed-off-by: Aditya Pachauri