optimization

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

Author: asarazin

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;
 

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"
@@ -151,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;
 
   /**

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -414,24 +414,19 @@ 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::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, ""};
   // 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
   for (std::shared_ptr<Source> source : sources_) {
+    std::vector<Point> collision_points;
+    auto iter = sources_collision_points_map.insert({source->getSourceName(), collision_points});
+    bool is_source_valid = source->getData(curr_time, iter.first->second);
+
     if (source->getEnabled()) {
-      if (!source->getData(curr_time, collision_points) &&
-        source->getSourceTimeout().seconds() != 0.0)
-      {
+      if (!is_source_valid && source->getSourceTimeout().seconds() != 0.0) {
         action_polygon = nullptr;
         robot_action.polygon_name = "invalid source";
         robot_action.action_type = STOP;
@@ -441,34 +436,34 @@ 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
-    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);
+    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 : 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);
+      collision_points_marker_pub_->publish(std::move(marker_array));
     }
-    marker_array->markers.push_back(marker);
-    collision_points_marker_pub_->publish(std::move(marker_array));
   }
 
   for (std::shared_ptr<Polygon> polygon : polygons_) {
@@ -483,37 +478,17 @@ 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
-      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;
       }
     }
@@ -535,15 +510,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();
@@ -590,7 +565,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
 {
@@ -599,7 +574,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();

nav2_collision_monitor/src/polygon.cpp

@@ -234,39 +234,80 @@ 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();
+
+  for (const auto & iter : sources_collision_points_map) {
+    // Check the points only if the source is associated with current polygon
+    if (std::find(
+        polygon_sources_names.begin(), polygon_sources_names.end(), iter.first) ==
+      polygon_sources_names.end())
+    {
+      continue;
+    }
+
+    // Sum the number of points from all sources associated with polygon
+    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;
 
-  // Array of points transformed to the frame concerned with pose on each simulation step
-  std::vector<Point> points_transformed = collision_points;
+  double collision_time = -1.0;  // initialize collision time as if there is no collision
+  std::vector<std::string> polygon_sources_names = getSourcesNames();
 
-  // Check static polygon
-  if (getPointsInside(points_transformed) >= min_points_) {
-    return 0.0;
-  }
+  for (const auto & iter : sources_collision_points_map) {
+    // Check the points only if the source is associated with current polygon
+    if (std::find(
+        polygon_sources_names.begin(), polygon_sources_names.end(), iter.first) ==
+      polygon_sources_names.end())
+    {
+      continue;
+    }
 
-  // Robot movement simulation
-  for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) {
-    // Shift the robot pose towards to the vel during simulation_time_step_ time interval
-    // NOTE: vel is changing during the simulation
-    projectState(simulation_time_step_, pose, vel);
-    // Transform collision_points to the frame concerned with current robot pose
-    points_transformed = collision_points;
-    transformPoints(pose, points_transformed);
-    // If the collision occurred on this stage, return the actual time before a collision
-    // as if robot was moved with given velocity
-    if (getPointsInside(points_transformed) >= min_points_) {
-      return time;
+    // Array of points transformed to the frame concerned with pose on each simulation step
+    std::vector<Point> points_transformed = iter.second;
+
+    // Check static polygon
+    if (getPointsInside(iter.second) >= min_points_) {
+      return 0.0;  // immediate collision, other sources don't need to be checked
+    }
+
+    // Robot movement simulation
+    for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) {
+      // Shift the robot pose towards to the vel during simulation_time_step_ time interval
+      // NOTE: vel is changing during the simulation
+      projectState(simulation_time_step_, pose, vel);
+      // Transform collision_points to the frame concerned with current robot pose
+      points_transformed = iter.second;
+      transformPoints(pose, points_transformed);
+      // If the collision occurred on this stage, return the actual time before a collision
+      // as if robot was moved with given velocity
+      if (getPointsInside(points_transformed) >= min_points_) {
+        if (collision_time == -1.0) {
+          // If first collision detected, keep collision time value
+          collision_time = time;
+        } else if (time < collision_time) {
+          // Keep the smallest collision time value from all sources
+          collision_time = time;
+        }
+      }
     }
   }
 
   // There is no collision
-  return -1.0;
+  return collision_time;
 }
 
 void Polygon::publish()