Revert "Merge pull request #20 from logivations/collision-monitor-add-publish-action"

This reverts commit 8996e15, reversing
changes made to 617f60a.

Author: Tony Najjar

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -141,11 +141,6 @@ 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
@@ -174,18 +169,6 @@ 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
@@ -217,8 +200,6 @@ 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,7 +22,6 @@
 #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"
@@ -136,11 +135,6 @@ 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
    */
@@ -199,10 +193,6 @@ 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,8 +65,7 @@ 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
-  PUBLISH = 4  // Publish to a topic
+  APPROACH = 3  // Keep constant time interval before collision
 };
 
 /// @brief Action for robot

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -24,8 +24,6 @@
 
 #include "nav2_collision_monitor/kinematics.hpp"
 
-using namespace std::chrono_literals;
-
 namespace nav2_collision_monitor
 {
 
@@ -69,15 +67,6 @@ 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;
 }
 
@@ -394,34 +383,6 @@ 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,
@@ -477,13 +438,6 @@ 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
 {
@@ -503,10 +457,6 @@ 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

@@ -73,11 +73,6 @@ 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;
@@ -103,19 +98,13 @@ 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
@@ -233,27 +222,6 @@ 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();
@@ -274,8 +242,6 @@ 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;
@@ -350,15 +316,6 @@ 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);