From 7953bbfe5d141d64e25531935c4007298622c025 Mon Sep 17 00:00:00 2001
From: yenode <adi.pachauri.444@gmail.com>
Date: Sat, 14 Mar 2026 22:58:22 +0530
Subject: [PATCH] feat: publish collision string message during emergency stop
 (Resolves #159)

Signed-off-by: Aditya Pachauri <adi.pachauri.444@gmail.com>
---
 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 <tf2_ros/transform_broadcaster.h>
 
 #include <geometry_msgs/msg/transform_stamped.hpp>
+#include <std_msgs/msg/string.hpp>
 #include <rmf_fleet_msgs/msg/robot_mode.hpp>
 #include <rmf_fleet_msgs/msg/robot_state.hpp>
 #include <rmf_fleet_msgs/msg/path_request.hpp>
@@ -174,6 +175,7 @@ class SlotcarCommon
 
   std::shared_ptr<tf2_ros::TransformBroadcaster> _tf2_broadcaster;
   rclcpp::Publisher<rmf_fleet_msgs::msg::RobotState>::SharedPtr _robot_state_pub;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _collision_pub;
 
   rclcpp::Subscription<rmf_fleet_msgs::msg::PathRequest>::SharedPtr _traj_sub;
   rclcpp::Subscription<rmf_fleet_msgs::msg::PauseRequest>::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 @@
   <depend>eigen</depend>
   <depend>rclcpp</depend>
   <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>rmf_fleet_msgs</depend>
   <depend>rmf_building_map_msgs</depend>
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<rmf_fleet_msgs::msg::RobotState>(
     "/robot_state", 10);
 
+  _collision_pub =
+    _ros_node->create_publisher<std_msgs::msg::String>(
+    "/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 << "]");