Collision monitor: select specific observation sources for polygon

Signed-off-by: asarazin <[email protected]>

Author: asarazin

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

@@ -126,6 +126,12 @@ class Polygon
    */
   virtual void getPolygon(std::vector<Point> & poly) const;
 
+  /**
+   * @brief Obtains the name of the observation sources for current polygon.
+   * @return Names of the observation sources
+   */
+  std::vector<std::string> getSourcesNames() const;
+
   /**
    * @brief Returns true if polygon points were set.
    * Othewise, prints a warning and returns false.
@@ -252,6 +258,8 @@ class Polygon
   rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_sub_;
   /// @brief Footprint subscriber
   std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
+  /// @brief Name of the observation sources to check for polygon
+  std::vector<std::string> sources_names_;
 
   // Global variables
   /// @brief TF buffer

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -270,17 +270,17 @@ bool CollisionMonitor::getParameters(
   stop_pub_timeout_ =
     rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double());
 
-  if (!configurePolygons(base_frame_id, transform_tolerance)) {
-    return false;
-  }
-
   if (
     !configureSources(
       base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction))
   {
     return false;
   }
 
+  if (!configurePolygons(base_frame_id, transform_tolerance)) {
+    return false;
+  }
+
   return true;
 }
 
@@ -405,7 +405,14 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
   }
 
   // Points array collected from different data sources in a robot base frame
-  std::vector<Point> collision_points;
+  std::unordered_map<std::string, std::vector<Point>> sources_collision_points_map;
+
+  // Fill collision_points array from different data sources
+  for (std::shared_ptr<Source> source : sources_) {
+    std::vector<Point> collision_points;
+    source->getData(curr_time, collision_points);
+    sources_collision_points_map.insert({source->getSourceName(), collision_points});
+  }
 
   // By default - there is no action
   Action robot_action{DO_NOTHING, cmd_vel_in, ""};
@@ -468,6 +475,28 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
     // Update polygon coordinates
     polygon->updatePolygon(cmd_vel_in);
 
+    // Get collision points for sources associated to polygon
+    std::vector<std::string> sources_names = polygon->getSourcesNames();
+    std::vector<Point> collision_points;
+    for (auto source_name : sources_names) {
+      try {
+        // Get vector for source
+        auto source_collision_points = sources_collision_points_map.at(source_name);
+        // Concatenate vectors
+        collision_points.insert(
+          collision_points.end(),
+          source_collision_points.begin(),
+          source_collision_points.end());
+      } catch (std::exception & e) {
+        RCLCPP_ERROR_STREAM_THROTTLE(
+          get_logger(),
+          *get_clock(),
+          1000,
+          "Observation source [" << source_name <<
+            "] configured for polygon [" << polygon->getName() << "] is not defined!");
+      }
+    }
+    
     const ActionType at = polygon->getActionType();
     if (at == STOP || at == SLOWDOWN || at == LIMIT) {
       // Process STOP/SLOWDOWN for the selected polygon

nav2_collision_monitor/src/polygon.cpp

@@ -168,6 +168,11 @@ double Polygon::getTimeBeforeCollision() const
   return time_before_collision_;
 }
 
+std::vector<std::string> Polygon::getSourcesNames() const
+{
+  return sources_names_;
+}
+
 void Polygon::getPolygon(std::vector<Point> & poly) const
 {
   poly = poly_;
@@ -372,6 +377,27 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
         node, polygon_name_ + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name_));
       polygon_pub_topic = node->get_parameter(polygon_name_ + ".polygon_pub_topic").as_string();
     }
+
+    // By default, use all observation sources for polygon
+    nav2_util::declare_parameter_if_not_declared(
+      node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY);
+    std::vector<std::string> source_names =
+      node->get_parameter("observation_sources").as_string_array();
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".sources_names", rclcpp::ParameterValue(source_names));
+    sources_names_ = node->get_parameter(polygon_name_ + ".sources_names").as_string_array();
+
+    // Check the observation sources configured for polygon are defined
+    for (auto source_name : sources_names_) {
+      if (std::find(source_names.begin(), source_names.end(), source_name) == source_names.end()) {
+        RCLCPP_ERROR_STREAM(
+          logger_,
+          "Observation source [" << source_name <<
+            "] configured for polygon [" << getName() <<
+            "] is not defined!");
+        return false;
+      }
+    }
   } catch (const std::exception & ex) {
     RCLCPP_ERROR(
       logger_,