From 1c672fac2fdf4b3d78b2f900e10b4cd4f7d4ec44 Mon Sep 17 00:00:00 2001 From: asarazin Date: Fri, 7 Jun 2024 16:48:06 +0200 Subject: [PATCH] optimization Signed-off-by: asarazin --- .../collision_monitor_node.hpp | 11 +- .../nav2_collision_monitor/polygon.hpp | 17 ++- .../src/collision_monitor_node.cpp | 105 +++++++----------- nav2_collision_monitor/src/polygon.cpp | 81 ++++++++++---- nav2_collision_monitor/test/polygons_test.cpp | 23 ++-- 5 files changed, 135 insertions(+), 102 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 index ce4d96504bb..c6caeeb5b2d 100644 --- 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 #include #include +#include #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, - const std::vector & collision_points, + const std::unordered_map> & 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, - const std::vector & collision_points, + const std::unordered_map> & 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 index fe132fcd798..6bb3b5dd011 100644 --- 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 #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" @@ -151,16 +152,28 @@ class Polygon */ virtual int getPointsInside(const std::vector & 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> & 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 & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity) const; /** diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 20ff3924c0e..576e1897e79 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/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> sources_collision_points_map; - // Fill collision_points array from different data sources - for (std::shared_ptr source : sources_) { - std::vector 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 action_polygon; - // Fill collision_points array from different data sources + // Fill collision points array from different data sources for (std::shared_ptr source : sources_) { + std::vector 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::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::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 : 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 sources_names = polygon->getSourcesNames(); - std::vector 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,7 +510,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: bool CollisionMonitor::processStopSlowdownLimit( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const { @@ -543,7 +518,7 @@ bool CollisionMonitor::processStopSlowdownLimit( 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, - const std::vector & collision_points, + const std::unordered_map> & 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(); diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index ddef394305e..4814765d5c1 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -234,39 +234,80 @@ int Polygon::getPointsInside(const std::vector & points) const return num; } +int Polygon::getPointsInside( + const std::unordered_map> & sources_collision_points_map) const +{ + int num = 0; + std::vector 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 & collision_points, + const std::unordered_map> & 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 points_transformed = collision_points; + double collision_time = -1.0; // initialize collision time as if there is no collision + std::vector 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 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() diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index c703e12557b..699783a4d2e 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -862,23 +862,24 @@ TEST_F(Tester, testPolygonGetCollisionTime) // Forward movement check nav2_collision_monitor::Velocity vel{0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points 0.2 m ahead the footprint (0.5 m) - std::vector points{{0.7, -0.01}, {0.7, 0.01}}; + std::unordered_map> sources_collision_points_map; + sources_collision_points_map.insert({"source", {{0.7, -0.01}, {0.7, 0.01}}}); // Collision is expected to be ~= 0.2 m / 0.5 m/s seconds EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP); // Backward movement check vel = {-0.5, 0.0, 0.0}; // 0.5 m/s backward movement // Two points 0.2 m behind the footprint (0.5 m) - points.clear(); - points = {{-0.7, -0.01}, {-0.7, 0.01}}; + sources_collision_points_map.clear(); + sources_collision_points_map.insert({"source", {{-0.7, -0.01}, {-0.7, 0.01}}}); // Collision is expected to be in ~= 0.2 m / 0.5 m/s seconds EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP); // Sideway movement check vel = {0.0, 0.5, 0.0}; // 0.5 m/s sideway movement // Two points 0.1 m ahead the footprint (0.5 m) - points.clear(); - points = {{-0.01, 0.6}, {0.01, 0.6}}; + sources_collision_points_map.clear(); + sources_collision_points_map.insert({"source", {{-0.01, 0.6}, {0.01, 0.6}}}); // Collision is expected to be in ~= 0.1 m / 0.5 m/s seconds EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.2, SIMULATION_TIME_STEP); @@ -894,8 +895,8 @@ TEST_F(Tester, testPolygonGetCollisionTime) // | ' | // ----------- // ' - points.clear(); - points = {{0.49, -0.01}, {0.49, 0.01}}; + sources_collision_points_map.clear(); + sources_collision_points_map.insert({"source", {{0.49, -0.01}, {0.49, 0.01}}}); // Collision is expected to be in ~= 45 degrees * M_PI / (180 degrees * 1.0 rad/s) seconds double exp_res = 45 / 180 * M_PI; EXPECT_NEAR(polygon_->getCollisionTime(points, vel), exp_res, EPSILON); @@ -903,16 +904,16 @@ TEST_F(Tester, testPolygonGetCollisionTime) // Two points are already inside footprint vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points inside - points.clear(); - points = {{0.1, -0.01}, {0.1, 0.01}}; + sources_collision_points_map.clear(); + sources_collision_points_map.insert({"source", {{0.1, -0.01}, {0.1, 0.01}}}); // Collision already appeared: collision time should be 0 EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.0, EPSILON); // All points are out of simulation prediction vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points 0.6 m ahead the footprint (0.5 m) - points.clear(); - points = {{1.1, -0.01}, {1.1, 0.01}}; + sources_collision_points_map.clear(); + sources_collision_points_map.insert({"source", {{1.1, -0.01}, {1.1, 0.01}}}); // There is no collision: return value should be negative EXPECT_LT(polygon_->getCollisionTime(points, vel), 0.0); }