[collision monitor] Select the observation sources used with each pol…

…ygon (ros-navigation#4227) * Collision monitor: select specific observation sources for polygon Signed-off-by: asarazin <[email protected]> * optimization Signed-off-by: asarazin <[email protected]> * add tests Signed-off-by: asarazin <[email protected]> --------- Signed-off-by: asarazin <[email protected]> Co-authored-by: asarazin <[email protected]> Signed-off-by: Joseph Duchesne <[email protected]>
josephduchesne · Dec 10, 2024 · 6d3507f · 6d3507f
1 parent 0d7044d
commit 6d3507f
Show file tree

Hide file tree

Showing 8 changed files with 280 additions and 71 deletions.
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
@@ -18,6 +18,7 @@
 #include <string>
 #include <vector>
 #include <memory>
+#include <unordered_map>
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/twist.hpp"
@@ -159,28 +160,30 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   /**
    * @brief Processes the polygon of STOP, SLOWDOWN and LIMIT action type
    * @param polygon Polygon to process
-   * @param collision_points Array of 2D obstacle points
+   * @param sources_collision_points_map Map containing source name as key and
+   * array of source's 2D obstacle points as value
    * @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 processStopSlowdownLimit(
     const std::shared_ptr<Polygon> polygon,
-    const std::vector<Point> & collision_points,
+    const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
     const Velocity & velocity,
     Action & robot_action) const;
 
   /**
    * @brief Processes APPROACH action type
    * @param polygon Polygon to process
-   * @param collision_points Array of 2D obstacle points
+   * @param sources_collision_points_map Map containing source name as key and
+   * array of source's 2D obstacle points as value
    * @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 processApproach(
     const std::shared_ptr<Polygon> polygon,
-    const std::vector<Point> & collision_points,
+    const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
     const Velocity & velocity,
     Action & robot_action) const;
 

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
@@ -18,6 +18,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <unordered_map>
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/polygon_stamped.hpp"
@@ -126,6 +127,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.
    * Otherwise, prints a warning and returns false.
@@ -145,16 +152,28 @@ class Polygon
    */
   virtual int getPointsInside(const std::vector<Point> & points) const;
 
+  /**
+   * @brief Gets number of points inside given polygon
+   * @param sources_collision_points_map Map containing source name as key,
+   * and input array of source's points to be checked as value
+   * @return Number of points inside polygon,
+   * for sources in map that are associated with current polygon.
+   * If there are no points, returns zero value.
+   */
+  virtual int getPointsInside(
+    const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map) const;
+
   /**
    * @brief Obtains estimated (simulated) time before a collision.
    * Applicable for APPROACH model.
-   * @param collision_points Array of 2D obstacle points
+   * @param sources_collision_points_map Map containing source name as key,
+   * and input array of source's 2D obstacle points as value
    * @param velocity Simulated robot velocity
    * @return Estimated time before a collision. If there is no collision,
    * return value will be negative.
    */
   double getCollisionTime(
-    const std::vector<Point> & collision_points,
+    const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
     const Velocity & velocity) const;
 
   /**
@@ -269,6 +288,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

diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp
@@ -168,17 +168,17 @@ bool CollisionDetector::getParameters()
   const bool base_shift_correction =
     get_parameter("base_shift_correction").as_bool();
 
-  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;
 }
 

diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/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;
 }
 
@@ -412,17 +412,21 @@ 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;
 
   // By default - there is no action
   Action robot_action{DO_NOTHING, cmd_vel_in, ""};
   // Polygon causing robot action (if any)
   std::shared_ptr<Polygon> action_polygon;
 
-  // Fill collision_points array from different data sources
+  // Fill collision points array from different data sources
+  auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
   for (std::shared_ptr<Source> source : sources_) {
+    auto iter = sources_collision_points_map.insert(
+      {source->getSourceName(), std::vector<Point>()});
+
     if (source->getEnabled()) {
-      if (!source->getData(curr_time, collision_points) &&
+      if (!source->getData(curr_time, iter.first->second) &&
         source->getSourceTimeout().seconds() != 0.0)
       {
         action_polygon = nullptr;
@@ -434,33 +438,35 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
         break;
       }
     }
+
+    if (collision_points_marker_pub_->get_subscription_count() > 0) {
+      // visualize collision points with markers
+      visualization_msgs::msg::Marker marker;
+      marker.header.frame_id = get_parameter("base_frame_id").as_string();
+      marker.header.stamp = rclcpp::Time(0, 0);
+      marker.ns = "collision_points_" + source->getSourceName();
+      marker.id = 0;
+      marker.type = visualization_msgs::msg::Marker::POINTS;
+      marker.action = visualization_msgs::msg::Marker::ADD;
+      marker.scale.x = 0.02;
+      marker.scale.y = 0.02;
+      marker.color.r = 1.0;
+      marker.color.a = 1.0;
+      marker.lifetime = rclcpp::Duration(0, 0);
+      marker.frame_locked = true;
+
+      for (const auto & point : iter.first->second) {
+        geometry_msgs::msg::Point p;
+        p.x = point.x;
+        p.y = point.y;
+        p.z = 0.0;
+        marker.points.push_back(p);
+      }
+      marker_array->markers.push_back(marker);
+    }
   }
 
   if (collision_points_marker_pub_->get_subscription_count() > 0) {
-    // visualize collision points with markers
-    auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
-    visualization_msgs::msg::Marker marker;
-    marker.header.frame_id = get_parameter("base_frame_id").as_string();
-    marker.header.stamp = rclcpp::Time(0, 0);
-    marker.ns = "collision_points";
-    marker.id = 0;
-    marker.type = visualization_msgs::msg::Marker::POINTS;
-    marker.action = visualization_msgs::msg::Marker::ADD;
-    marker.scale.x = 0.02;
-    marker.scale.y = 0.02;
-    marker.color.r = 1.0;
-    marker.color.a = 1.0;
-    marker.lifetime = rclcpp::Duration(0, 0);
-    marker.frame_locked = true;
-
-    for (const auto & point : collision_points) {
-      geometry_msgs::msg::Point p;
-      p.x = point.x;
-      p.y = point.y;
-      p.z = 0.0;
-      marker.points.push_back(p);
-    }
-    marker_array->markers.push_back(marker);
     collision_points_marker_pub_->publish(std::move(marker_array));
   }
 
@@ -479,12 +485,14 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
     const ActionType at = polygon->getActionType();
     if (at == STOP || at == SLOWDOWN || at == LIMIT) {
       // Process STOP/SLOWDOWN for the selected polygon
-      if (processStopSlowdownLimit(polygon, collision_points, cmd_vel_in, robot_action)) {
+      if (processStopSlowdownLimit(
+          polygon, sources_collision_points_map, cmd_vel_in, robot_action))
+      {
         action_polygon = polygon;
       }
     } else if (at == APPROACH) {
       // Process APPROACH for the selected polygon
-      if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) {
+      if (processApproach(polygon, sources_collision_points_map, cmd_vel_in, robot_action)) {
         action_polygon = polygon;
       }
     }
@@ -506,15 +514,15 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
 
 bool CollisionMonitor::processStopSlowdownLimit(
   const std::shared_ptr<Polygon> polygon,
-  const std::vector<Point> & collision_points,
+  const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
   const Velocity & velocity,
   Action & robot_action) const
 {
   if (!polygon->isShapeSet()) {
     return false;
   }
 
-  if (polygon->getPointsInside(collision_points) >= polygon->getMinPoints()) {
+  if (polygon->getPointsInside(sources_collision_points_map) >= polygon->getMinPoints()) {
     if (polygon->getActionType() == STOP) {
       // Setting up zero velocity for STOP model
       robot_action.polygon_name = polygon->getName();
@@ -561,7 +569,7 @@ bool CollisionMonitor::processStopSlowdownLimit(
 
 bool CollisionMonitor::processApproach(
   const std::shared_ptr<Polygon> polygon,
-  const std::vector<Point> & collision_points,
+  const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
   const Velocity & velocity,
   Action & robot_action) const
 {
@@ -570,7 +578,7 @@ bool CollisionMonitor::processApproach(
   }
 
   // Obtain time before a collision
-  const double collision_time = polygon->getCollisionTime(collision_points, velocity);
+  const double collision_time = polygon->getCollisionTime(sources_collision_points_map, velocity);
   if (collision_time >= 0.0) {
     // If collision will occurr, reduce robot speed
     const double change_ratio = collision_time / polygon->getTimeBeforeCollision();

diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp
@@ -157,6 +157,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_;
@@ -229,19 +234,48 @@ int Polygon::getPointsInside(const std::vector<Point> & points) const
   return num;
 }
 
+int Polygon::getPointsInside(
+  const std::unordered_map<std::string,
+  std::vector<Point>> & sources_collision_points_map) const
+{
+  int num = 0;
+  std::vector<std::string> polygon_sources_names = getSourcesNames();
+
+  // Sum the number of points from all sources associated with current polygon
+  for (const auto & source_name : polygon_sources_names) {
+    const auto & iter = sources_collision_points_map.find(source_name);
+    if (iter != sources_collision_points_map.end()) {
+      num += getPointsInside(iter->second);
+    }
+  }
+
+  return num;
+}
+
 double Polygon::getCollisionTime(
-  const std::vector<Point> & collision_points,
+  const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
   const Velocity & velocity) const
 {
   // Initial robot pose is {0,0} in base_footprint coordinates
   Pose pose = {0.0, 0.0, 0.0};
   Velocity vel = velocity;
 
+  std::vector<std::string> polygon_sources_names = getSourcesNames();
+  std::vector<Point> collision_points;
+
+  // Save all points coming from the sources associated with current polygon
+  for (const auto & source_name : polygon_sources_names) {
+    const auto & iter = sources_collision_points_map.find(source_name);
+    if (iter != sources_collision_points_map.end()) {
+      collision_points.insert(collision_points.end(), iter->second.begin(), iter->second.end());
+    }
+  }
+
   // Array of points transformed to the frame concerned with pose on each simulation step
   std::vector<Point> points_transformed = collision_points;
 
   // Check static polygon
-  if (getPointsInside(points_transformed) >= min_points_) {
+  if (getPointsInside(collision_points) >= min_points_) {
     return 0.0;
   }
 
@@ -392,6 +426,29 @@ bool Polygon::getCommonParameters(
           node->get_parameter(polygon_name_ + ".footprint_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);
+    const std::vector<std::string> observation_sources =
+      node->get_parameter("observation_sources").as_string_array();
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".sources_names", rclcpp::ParameterValue(observation_sources));
+    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(observation_sources.begin(), observation_sources.end(), source_name) ==
+        observation_sources.end())
+      {
+        RCLCPP_ERROR_STREAM(
+          logger_,
+          "Observation source [" << source_name <<
+            "] configured for polygon [" << getName() <<
+            "] is not defined as one of the node's observation_source!");
+        return false;
+      }
+    }
   } catch (const std::exception & ex) {
     RCLCPP_ERROR(
       logger_,