feat: publish collision string message during emergency stop (Resolves #159)

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

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 <rmf_fleet_msgs/msg/robot_collision.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<rmf_fleet_msgs::msg::RobotCollision>::SharedPtr _collision_pub;
 
   rclcpp::Subscription<rmf_fleet_msgs::msg::PathRequest>::SharedPtr _traj_sub;
   rclcpp::Subscription<rmf_fleet_msgs::msg::PauseRequest>::SharedPtr _pause_sub;

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>

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<rmf_fleet_msgs::msg::RobotCollision>(
+    "/robot_collisions", 10);
+
   auto qos_profile = rclcpp::QoS(10);
   qos_profile.transient_local();
   _building_map_sub =
@@ -842,8 +846,25 @@ 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");
+
+      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
       RCLCPP_INFO_STREAM(logger(), "No more obstacles; resuming course for [" <<
         _model_name << "]");