Add Publish Action

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -141,6 +141,11 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    */
   void process(const Velocity & cmd_vel_in);
 
+  /**
+   * @brief Timer callback for actions not requiring vel
+   */
+  void process_without_vel();
+
   /**
    * @brief Processes the polygon of STOP and SLOWDOWN action type
    * @param polygon Polygon to process
@@ -169,6 +174,18 @@ class CollisionMonitor : public nav2_util::LifecycleNode
     const Velocity & velocity,
     Action & robot_action) const;
 
+  /**
+   * @brief Processes Publish action type
+   * @param polygon Polygon to process
+   * @param collision_points Array of 2D obstacle points
+   * @param velocity Desired robot velocity
+   * @param robot_action Output processed robot action
+   * @return True if returned action is caused by current polygon, otherwise false
+   */
+  bool processPublish(
+    const std::shared_ptr<Polygon> polygon,
+    const std::vector<Point> & collision_points) const;
+
   /**
    * @brief Prints robot action and polygon caused it (if it was)
    * @param robot_action Robot action to print
@@ -200,6 +217,8 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_in_sub_;
   /// @brief Output cmd_vel publisher
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_out_pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
 
   /// @brief Whether main routine is active
   bool process_active_;

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

@@ -22,6 +22,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/polygon_stamped.hpp"
 #include "geometry_msgs/msg/polygon.hpp"
+#include "std_msgs/msg/bool.hpp"
 
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
@@ -135,6 +136,11 @@ class Polygon
     const std::vector<Point> & collision_points,
     const Velocity & velocity) const;
 
+  /**
+   * @brief Publishes detection message
+   */
+  bool publish_detection(std::vector<Point> collision_points) const;
+
   /**
    * @brief Publishes polygon message into a its own topic
    */
@@ -185,6 +191,10 @@ class Polygon
   double simulation_time_step_;
   /// @brief Footprint subscriber
   std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
+  /// @brief Detection topic
+  std::string detection_topic_;
+  /// @brief Detection publisher
+  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Bool>::SharedPtr detection_pub_;
 
   // Global variables
   /// @brief TF buffer

nav2_collision_monitor/include/nav2_collision_monitor/types.hpp

@@ -65,7 +65,8 @@ enum ActionType
   DO_NOTHING = 0,  // No action
   STOP = 1,  // Stop the robot
   SLOWDOWN = 2,  // Slowdown in percentage from current operating speed
-  APPROACH = 3  // Keep constant time interval before collision
+  APPROACH = 3,  // Keep constant time interval before collision
+  PUBLISH = 4  // Publish to a topic
 };
 
 /// @brief Action for robot

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -24,6 +24,8 @@
 
 #include "nav2_collision_monitor/kinematics.hpp"
 
+using namespace std::chrono_literals;
+
 namespace nav2_collision_monitor
 {
 
@@ -67,6 +69,15 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
   cmd_vel_out_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
     cmd_vel_out_topic, 1);
 
+  for (std::shared_ptr<Polygon> polygon : polygons_) {
+    if (polygon->getActionType() == PUBLISH) {
+      timer_ = this->create_wall_timer(
+        100ms,
+        std::bind(&CollisionMonitor::process_without_vel, this));
+      break;
+    }
+  }
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -383,6 +394,34 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
   robot_action_prev_ = robot_action;
 }
 
+void CollisionMonitor::process_without_vel()
+{
+  // Current timestamp for all inner routines prolongation
+  rclcpp::Time curr_time = this->now();
+
+  // Do nothing if main worker in non-active state
+  if (!process_active_) {
+    return;
+  }
+
+  // Points array collected from different data sources in a robot base frame
+  std::vector<Point> collision_points;
+
+  // Fill collision_points array from different data sources
+  for (std::shared_ptr<Source> source : sources_) {
+    source->getData(curr_time, collision_points);
+  }
+
+  for (std::shared_ptr<Polygon> polygon : polygons_) {
+    const ActionType at = polygon->getActionType();
+    if (at == PUBLISH) {
+      // Process PUBLISH for the selected polygon
+      processPublish(polygon, collision_points);
+    }
+  }
+  publishPolygons();
+}
+
 bool CollisionMonitor::processStopSlowdown(
   const std::shared_ptr<Polygon> polygon,
   const std::vector<Point> & collision_points,
@@ -438,6 +477,13 @@ bool CollisionMonitor::processApproach(
   return false;
 }
 
+bool CollisionMonitor::processPublish(
+  const std::shared_ptr<Polygon> polygon,
+  const std::vector<Point> & collision_points) const
+{
+  return polygon->publish_detection(collision_points);
+}
+
 void CollisionMonitor::printAction(
   const Action & robot_action, const std::shared_ptr<Polygon> action_polygon) const
 {
@@ -457,6 +503,10 @@ void CollisionMonitor::printAction(
       get_logger(),
       "Robot to approach for %f seconds away from collision",
       action_polygon->getTimeBeforeCollision());
+  } else if (robot_action.action_type == PUBLISH) {
+    RCLCPP_INFO(
+      get_logger(),
+      "Robot to publish to configured topic collision detection");
   } else {  // robot_action.action_type == DO_NOTHING
     RCLCPP_INFO(
       get_logger(),

nav2_collision_monitor/src/polygon.cpp

@@ -65,6 +65,11 @@ bool Polygon::configure()
       base_frame_id_, tf2::durationToSec(transform_tolerance_));
   }
 
+  if (!detection_topic_.empty()) {
+    detection_pub_ = node->create_publisher<std_msgs::msg::Bool>(
+    detection_topic_, rclcpp::SystemDefaultsQoS());
+  }
+
   if (visualize_) {
     // Fill polygon_ points for future usage
     std::vector<Point> poly;
@@ -90,13 +95,19 @@ void Polygon::activate()
   if (visualize_) {
     polygon_pub_->on_activate();
   }
+  if (!detection_topic_.empty()) {
+    detection_pub_->on_activate();
+  }
 }
 
 void Polygon::deactivate()
 {
   if (visualize_) {
     polygon_pub_->on_deactivate();
   }
+  if (!detection_topic_.empty()) {
+    detection_pub_->on_deactivate();
+  }
 }
 
 std::string Polygon::getName() const
@@ -214,6 +225,27 @@ void Polygon::publish() const
   polygon_pub_->publish(std::move(poly_s));
 }
 
+bool Polygon::publish_detection(std::vector<Point> collision_points) const
+{
+  if (detection_topic_.empty()) {
+    return false;
+  }
+
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+  bool detected = getPointsInside(collision_points) > getMaxPoints();
+
+  std::unique_ptr<std_msgs::msg::Bool> detection =
+    std::make_unique<std_msgs::msg::Bool>();
+  detection->data = detected;
+  // Publish polygon
+
+  detection_pub_->publish(std::move(detection));
+  return detected;
+}
+
 bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
 {
   auto node = node_.lock();
@@ -234,6 +266,8 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
       action_type_ = SLOWDOWN;
     } else if (at_str == "approach") {
       action_type_ = APPROACH;
+    } else if (at_str == "publish") {
+      action_type_ = PUBLISH;
     } else {  // Error if something else
       RCLCPP_ERROR(logger_, "[%s]: Unknown action type: %s", polygon_name_.c_str(), at_str.c_str());
       return false;
@@ -308,6 +342,15 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp
       footprint_topic.clear();
     }
 
+    if (action_type_ == PUBLISH) {
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".detection_topic", rclcpp::ParameterValue(polygon_name_));
+      detection_topic_ =
+        node->get_parameter(polygon_name_ + ".detection_topic").as_string();
+    } else {
+      detection_topic_.clear();
+    }
+
     // Leave it not initialized: the will cause an error if it will not set
     nav2_util::declare_parameter_if_not_declared(
       node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY);