Publish collision monitor state (#3504)

nav2_collision_monitor/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_msgs REQUIRED)
 
 ### Header ###
 
@@ -35,6 +36,7 @@ set(dependencies
   tf2_geometry_msgs
   nav2_util
   nav2_costmap_2d
+  nav2_msgs
 )
 
 set(executable_name collision_monitor)

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -28,6 +28,7 @@
 
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_util/robot_utils.hpp"
+#include "nav2_msgs/msg/collision_monitor_state.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/polygon.hpp"
@@ -111,7 +112,8 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    */
   bool getParameters(
     std::string & cmd_vel_in_topic,
-    std::string & cmd_vel_out_topic);
+    std::string & cmd_vel_out_topic,
+    std::string & state_topic);
   /**
    * @brief Supporting routine creating and configuring all polygons
    * @param base_frame_id Robot base frame ID
@@ -174,11 +176,11 @@ class CollisionMonitor : public nav2_util::LifecycleNode
     Action & robot_action) const;
 
   /**
-   * @brief Prints robot action and polygon caused it (if it was)
-   * @param robot_action Robot action to print
+   * @brief Log and publish current robot action and polygon
+   * @param robot_action Robot action to notify
    * @param action_polygon Pointer to a polygon causing a selected action
    */
-  void printAction(
+  void notifyActionState(
     const Action & robot_action, const std::shared_ptr<Polygon> action_polygon) const;
 
   /**
@@ -205,6 +207,10 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   /// @brief Output cmd_vel publisher
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_out_pub_;
 
+  /// @brief CollisionMonitor state publisher
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionMonitorState>::SharedPtr
+    state_pub_;
+
   /// @brief Whether main routine is active
   bool process_active_;
 

nav2_collision_monitor/include/nav2_collision_monitor/types.hpp

@@ -15,6 +15,8 @@
 #ifndef NAV2_COLLISION_MONITOR__TYPES_HPP_
 #define NAV2_COLLISION_MONITOR__TYPES_HPP_
 
+#include <string>
+
 namespace nav2_collision_monitor
 {
 
@@ -73,6 +75,7 @@ struct Action
 {
   ActionType action_type;
   Velocity req_vel;
+  std::string polygon_name;
 };
 
 }  // namespace nav2_collision_monitor

nav2_collision_monitor/package.xml

@@ -20,6 +20,7 @@
   <depend>nav2_common</depend>
   <depend>nav2_util</depend>
   <depend>nav2_costmap_2d</depend>
+  <depend>nav2_msgs</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>

nav2_collision_monitor/params/collision_monitor_params.yaml

@@ -5,6 +5,7 @@ collision_monitor:
     odom_frame_id: "odom"
     cmd_vel_in_topic: "cmd_vel_raw"
     cmd_vel_out_topic: "cmd_vel"
+    state_topic: "collision_monitor_state"
     transform_tolerance: 0.5
     source_timeout: 5.0
     base_shift_correction: True

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -29,7 +29,7 @@ namespace nav2_collision_monitor
 
 CollisionMonitor::CollisionMonitor(const rclcpp::NodeOptions & options)
 : nav2_util::LifecycleNode("collision_monitor", "", options),
-  process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}},
+  process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}, ""},
   stop_stamp_{0, 0, get_clock()->get_clock_type()}, stop_pub_timeout_(1.0, 0.0)
 {
 }
@@ -55,9 +55,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;
 
   // Obtaining ROS parameters
-  if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic)) {
+  if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) {
     return nav2_util::CallbackReturn::FAILURE;
   }
 
@@ -66,6 +67,10 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
     std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1));
   cmd_vel_out_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
     cmd_vel_out_topic, 1);
+  if (!state_topic.empty()) {
+    state_pub_ = this->create_publisher<nav2_msgs::msg::CollisionMonitorState>(
+      state_topic, 1);
+  }
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -77,6 +82,9 @@ CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/)
 
   // Activating lifecycle publisher
   cmd_vel_out_pub_->on_activate();
+  if (state_pub_) {
+    state_pub_->on_activate();
+  }
 
   // Activating polygons
   for (std::shared_ptr<Polygon> polygon : polygons_) {
@@ -105,7 +113,7 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   process_active_ = false;
 
   // Reset action type to default after worker deactivating
-  robot_action_prev_ = {DO_NOTHING, {-1.0, -1.0, -1.0}};
+  robot_action_prev_ = {DO_NOTHING, {-1.0, -1.0, -1.0}, ""};
 
   // Deactivating polygons
   for (std::shared_ptr<Polygon> polygon : polygons_) {
@@ -114,6 +122,9 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 
   // Deactivating lifecycle publishers
   cmd_vel_out_pub_->on_deactivate();
+  if (state_pub_) {
+    state_pub_->on_deactivate();
+  }
 
   // Destroying bond connection
   destroyBond();
@@ -128,6 +139,7 @@ CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 
   cmd_vel_in_sub_.reset();
   cmd_vel_out_pub_.reset();
+  state_pub_.reset();
 
   polygons_.clear();
   sources_.clear();
@@ -182,7 +194,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 & cmd_vel_out_topic,
+  std::string & state_topic)
 {
   std::string base_frame_id, odom_frame_id;
   tf2::Duration transform_tolerance;
@@ -196,6 +209,9 @@ bool CollisionMonitor::getParameters(
   nav2_util::declare_parameter_if_not_declared(
     node, "cmd_vel_out_topic", rclcpp::ParameterValue("cmd_vel"));
   cmd_vel_out_topic = get_parameter("cmd_vel_out_topic").as_string();
+  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, "base_frame_id", rclcpp::ParameterValue("base_footprint"));
@@ -359,7 +375,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
   }
 
   // By default - there is no action
-  Action robot_action{DO_NOTHING, cmd_vel_in};
+  Action robot_action{DO_NOTHING, cmd_vel_in, ""};
   // Polygon causing robot action (if any)
   std::shared_ptr<Polygon> action_polygon;
 
@@ -383,9 +399,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
     }
   }
 
-  if (robot_action.action_type != robot_action_prev_.action_type) {
+  if (robot_action.polygon_name != robot_action_prev_.polygon_name) {
     // Report changed robot behavior
-    printAction(robot_action, action_polygon);
+    notifyActionState(robot_action, action_polygon);
   }
 
   // Publish requred robot velocity
@@ -406,6 +422,7 @@ bool CollisionMonitor::processStopSlowdown(
   if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) {
     if (polygon->getActionType() == STOP) {
       // Setting up zero velocity for STOP model
+      robot_action.polygon_name = polygon->getName();
       robot_action.action_type = STOP;
       robot_action.req_vel.x = 0.0;
       robot_action.req_vel.y = 0.0;
@@ -416,6 +433,7 @@ bool CollisionMonitor::processStopSlowdown(
       // Check that currently calculated velocity is safer than
       // chosen for previous shapes one
       if (safe_vel < robot_action.req_vel) {
+        robot_action.polygon_name = polygon->getName();
         robot_action.action_type = SLOWDOWN;
         robot_action.req_vel = safe_vel;
         return true;
@@ -443,6 +461,7 @@ bool CollisionMonitor::processApproach(
     // Check that currently calculated velocity is safer than
     // chosen for previous shapes one
     if (safe_vel < robot_action.req_vel) {
+      robot_action.polygon_name = polygon->getName();
       robot_action.action_type = APPROACH;
       robot_action.req_vel = safe_vel;
       return true;
@@ -452,7 +471,7 @@ bool CollisionMonitor::processApproach(
   return false;
 }
 
-void CollisionMonitor::printAction(
+void CollisionMonitor::notifyActionState(
   const Action & robot_action, const std::shared_ptr<Polygon> action_polygon) const
 {
   if (robot_action.action_type == STOP) {
@@ -476,6 +495,15 @@ void CollisionMonitor::printAction(
       get_logger(),
       "Robot to continue normal operation");
   }
+
+  if (state_pub_) {
+    std::unique_ptr<nav2_msgs::msg::CollisionMonitorState> state_msg =
+      std::make_unique<nav2_msgs::msg::CollisionMonitorState>();
+    state_msg->polygon_name = robot_action.polygon_name;
+    state_msg->action_type = robot_action.action_type;
+
+    state_pub_->publish(std::move(state_msg));
+  }
 }
 
 void CollisionMonitor::publishPolygons() const