diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 788a77ebd4..134a52cff6 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -23,6 +23,7 @@ The following models of safety behaviors are employed by Collision Monitor: * **Stop model**: Define a zone and a point threshold. If more that `N` obstacle points appear inside this area, stop the robot until the obstacles will disappear. * **Slowdown model**: Define a zone around the robot and slow the maximum speed for a `%S` percent, if more than `N` points will appear inside the area. +* **Limit model**: Define a zone around the robot and clamp the maximum speed below a fixed value, if more than `N` points will appear inside the area. * **Approach model**: Using the current robot speed, estimate the time to collision to sensor data. If the time is less than `M` seconds (0.5, 2, 5, etc...), the robot will slow such that it is now at least `M` seconds to collision. The effect here would be to keep the robot always `M` seconds from any collision. The zones around the robot can take the following shapes: @@ -56,7 +57,7 @@ Designed to be used in wide variety of robots (incl. moving fast) and have a hig Typical one frame processing time is ~4-5ms for laser scanner (with 360 points) and ~4-20ms for PointClouds (having 24K points). The table below represents the details of operating times for different behavior models and shapes: -| | Stop/Slowdown model, Polygon area | Stop/Slowdown model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint | +| | Stop/Slowdown/Limit model, Polygon area | Stop/Slowdown/Limit model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint | |-|-----------------------------------|----------------------------------|-----------------------------------|----------------------------------| | LaserScan (360 points) processing time, ms | 4.45 | 4.45 | 4.93 | 4.86 | | PointCloud (24K points) processing time, ms | 4.94 | 4.06 | 20.67 | 10.87 | diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index 9cab7485be..c440032701 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -26,7 +26,7 @@ namespace nav2_collision_monitor /** * @brief Circle shape implementaiton. - * For STOP/SLOWDOWN model it represents zone around the robot + * For STOP/SLOWDOWN/LIMIT model it represents zone around the robot * while for APPROACH model it represents robot footprint. */ class Circle : public Polygon 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 adeab2daee..52934fc12d 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 @@ -147,14 +147,14 @@ class CollisionMonitor : public nav2_util::LifecycleNode void process(const Velocity & cmd_vel_in); /** - * @brief Processes the polygon of STOP and SLOWDOWN action type + * @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 velocity Desired robot velocity * @param robot_action Output processed robot action * @return True if returned action is caused by current polygon, otherwise false */ - bool processStopSlowdown( + bool processStopSlowdownLimit( const std::shared_ptr polygon, const std::vector & collision_points, const Velocity & velocity, diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 3580a45245..9ff6e09ee9 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -35,7 +35,7 @@ namespace nav2_collision_monitor /** * @brief Basic polygon shape class. - * For STOP/SLOWDOWN model it represents zone around the robot + * For STOP/SLOWDOWN/LIMIT model it represents zone around the robot * while for APPROACH model it represents robot footprint. */ class Polygon @@ -96,6 +96,18 @@ class Polygon * @return Speed slowdown ratio */ double getSlowdownRatio() const; + /** + * @brief Obtains speed linear limit for current polygon. + * Applicable for LIMIT model. + * @return Speed linear limit + */ + double getLinearLimit() const; + /** + * @brief Obtains speed angular z limit for current polygon. + * Applicable for LIMIT model. + * @return Speed angular limit + */ + double getAngularLimit() const; /** * @brief Obtains required time before collision for current polygon. * Applicable for APPROACH model. @@ -202,6 +214,10 @@ class Polygon int min_points_; /// @brief Robot slowdown (share of its actual speed) double slowdown_ratio_; + /// @brief Robot linear limit + double linear_limit_; + /// @brief Robot angular limit + double angular_limit_; /// @brief Time before collision in seconds double time_before_collision_; /// @brief Time step for robot movement simulation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp index 585faeafcb..ac93b916be 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp @@ -67,7 +67,8 @@ 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 + APPROACH = 3, // Keep constant time interval before collision + LIMIT = 4 // Limit absolute velocity from current operating speed }; /// @brief Action for robot diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 23a8b5548c..25bf9335f1 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -9,7 +9,7 @@ collision_monitor: source_timeout: 5.0 base_shift_correction: True stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop" and "slowdown" action types, + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, # and robot footprint for "approach" action type. # Footprint could be "polygon" type with dynamically set footprint from footprint_topic # or "circle" type with static footprint set by radius. "footprint_topic" parameter @@ -30,6 +30,15 @@ collision_monitor: slowdown_ratio: 0.3 visualize: True polygon_pub_topic: "polygon_slowdown" + PolygonLimit: + type: "polygon" + points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5] + action_type: "limit" + max_points: 3 + linear_limit: 0.4 + angular_limit: 0.5 + visualize: True + polygon_pub_topic: "polygon_limit" FootprintApproach: type: "polygon" action_type: "approach" diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index fb330ab43d..290ffb3c12 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -380,9 +380,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) } const ActionType at = polygon->getActionType(); - if (at == STOP || at == SLOWDOWN) { + if (at == STOP || at == SLOWDOWN || at == LIMIT) { // Process STOP/SLOWDOWN for the selected polygon - if (processStopSlowdown(polygon, collision_points, cmd_vel_in, robot_action)) { + if (processStopSlowdownLimit(polygon, collision_points, cmd_vel_in, robot_action)) { action_polygon = polygon; } } else if (at == APPROACH) { @@ -407,7 +407,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) robot_action_prev_ = robot_action; } -bool CollisionMonitor::processStopSlowdown( +bool CollisionMonitor::processStopSlowdownLimit( const std::shared_ptr polygon, const std::vector & collision_points, const Velocity & velocity, @@ -426,7 +426,7 @@ bool CollisionMonitor::processStopSlowdown( robot_action.req_vel.y = 0.0; robot_action.req_vel.tw = 0.0; return true; - } else { // SLOWDOWN + } else if (polygon->getActionType() == SLOWDOWN) { const Velocity safe_vel = velocity * polygon->getSlowdownRatio(); // Check that currently calculated velocity is safer than // chosen for previous shapes one @@ -436,6 +436,26 @@ bool CollisionMonitor::processStopSlowdown( robot_action.req_vel = safe_vel; return true; } + } else { // Limit + // Compute linear velocity + const double linear_vel = std::hypot(velocity.x, velocity.y); // absolute + Velocity safe_vel; + double ratio = 1.0; + if (linear_vel != 0.0) { + ratio = std::clamp(polygon->getLinearLimit() / linear_vel, 0.0, 1.0); + } + safe_vel.x = velocity.x * ratio; + safe_vel.y = velocity.y * ratio; + safe_vel.tw = std::clamp( + velocity.tw, -polygon->getAngularLimit(), polygon->getAngularLimit()); + // Check that currently calculated velocity is safer than + // chosen for previous shapes one + if (safe_vel < robot_action.req_vel) { + robot_action.polygon_name = polygon->getName(); + robot_action.action_type = LIMIT; + robot_action.req_vel = safe_vel; + return true; + } } } @@ -487,6 +507,11 @@ void CollisionMonitor::notifyActionState( "Robot to slowdown for %f percents due to %s polygon", action_polygon->getSlowdownRatio() * 100, action_polygon->getName().c_str()); + } else if (robot_action.action_type == LIMIT) { + RCLCPP_INFO( + get_logger(), + "Robot to limit speed due to %s polygon", + action_polygon->getName().c_str()); } else if (robot_action.action_type == APPROACH) { RCLCPP_INFO( get_logger(), diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index edde883c95..8af65d9159 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -35,7 +35,8 @@ Polygon::Polygon( const std::string & base_frame_id, const tf2::Duration & transform_tolerance) : node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING), - slowdown_ratio_(0.0), footprint_sub_(nullptr), tf_buffer_(tf_buffer), + slowdown_ratio_(0.0), linear_limit_(0.0), angular_limit_(0.0), + footprint_sub_(nullptr), tf_buffer_(tf_buffer), base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance) { RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str()); @@ -138,6 +139,16 @@ double Polygon::getSlowdownRatio() const return slowdown_ratio_; } +double Polygon::getLinearLimit() const +{ + return linear_limit_; +} + +double Polygon::getAngularLimit() const +{ + return angular_limit_; +} + double Polygon::getTimeBeforeCollision() const { return time_before_collision_; @@ -256,6 +267,8 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) action_type_ = STOP; } else if (at_str == "slowdown") { action_type_ = SLOWDOWN; + } else if (at_str == "limit") { + action_type_ = LIMIT; } else if (at_str == "approach") { action_type_ = APPROACH; } else { // Error if something else @@ -286,6 +299,15 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) slowdown_ratio_ = node->get_parameter(polygon_name_ + ".slowdown_ratio").as_double(); } + if (action_type_ == LIMIT) { + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".linear_limit", rclcpp::ParameterValue(0.5)); + linear_limit_ = node->get_parameter(polygon_name_ + ".linear_limit").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".angular_limit", rclcpp::ParameterValue(0.5)); + angular_limit_ = node->get_parameter(polygon_name_ + ".angular_limit").as_double(); + } + if (action_type_ == APPROACH) { nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".time_before_collision", rclcpp::ParameterValue(2.0)); @@ -374,7 +396,7 @@ bool Polygon::getParameters( polygon_name_.c_str()); } - if (action_type_ == STOP || action_type_ == SLOWDOWN) { + if (action_type_ == STOP || action_type_ == SLOWDOWN || action_type_ == LIMIT) { // Dynamic polygon will be used nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING); diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 5ba19d443c..5491e948e3 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -54,6 +54,8 @@ static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char RANGE_NAME[]{"Range"}; static const int MIN_POINTS{2}; static const double SLOWDOWN_RATIO{0.7}; +static const double LINEAR_LIMIT{0.4}; +static const double ANGULAR_LIMIT{0.09}; static const double TIME_BEFORE_COLLISION{1.0}; static const double SIMULATION_TIME_STEP{0.01}; static const double TRANSFORM_TOLERANCE{0.5}; @@ -81,6 +83,7 @@ enum ActionType STOP = 1, SLOWDOWN = 2, APPROACH = 3, + LIMIT = 4, }; class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor @@ -321,6 +324,16 @@ void Tester::addPolygon( cm_->set_parameter( rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO)); + cm_->declare_parameter( + polygon_name + ".linear_limit", rclcpp::ParameterValue(LINEAR_LIMIT)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".linear_limit", LINEAR_LIMIT)); + + cm_->declare_parameter( + polygon_name + ".angular_limit", rclcpp::ParameterValue(ANGULAR_LIMIT)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".angular_limit", ANGULAR_LIMIT)); + cm_->declare_parameter( polygon_name + ".time_before_collision", rclcpp::ParameterValue(TIME_BEFORE_COLLISION)); cm_->set_parameter( @@ -590,17 +603,18 @@ void Tester::actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPt action_state_ = msg; } -TEST_F(Tester, testProcessStopSlowdown) +TEST_F(Tester, testProcessStopSlowdownLimit) { rclcpp::Time curr_time = cm_->now(); // Set Collision Monitor parameters. // Making two polygons: outer polygon for slowdown and inner for robot stop. setCommonParameters(); + addPolygon("Limit", POLYGON, 3.0, "limit"); addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); addPolygon("Stop", POLYGON, 1.0, "stop"); addSource(SCAN_NAME, SCAN); - setVectors({"SlowDown", "Stop"}, {SCAN_NAME}); + setVectors({"Limit", "SlowDown", "Stop"}, {SCAN_NAME}); // Start Collision Monitor node cm_->start(); @@ -609,15 +623,29 @@ TEST_F(Tester, testProcessStopSlowdown) sendTransforms(curr_time); // 1. Obstacle is far away from robot - publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); + publishScan(4.5, curr_time); + ASSERT_TRUE(waitData(4.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); - // 2. Obstacle is in slowdown robot zone + // 2. Obstacle is in limit robot zone + publishScan(3.0, curr_time); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2); + const double ratio = LINEAR_LIMIT / speed; + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, LIMIT); + ASSERT_EQ(action_state_->polygon_name, "Limit"); + + // 3. Obstacle is in slowdown robot zone publishScan(1.5, curr_time); ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); @@ -629,7 +657,7 @@ TEST_F(Tester, testProcessStopSlowdown) ASSERT_EQ(action_state_->action_type, SLOWDOWN); ASSERT_EQ(action_state_->polygon_name, "SlowDown"); - // 3. Obstacle is inside stop zone + // 4. Obstacle is inside stop zone publishScan(0.5, curr_time); ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); @@ -641,9 +669,9 @@ TEST_F(Tester, testProcessStopSlowdown) ASSERT_EQ(action_state_->action_type, STOP); ASSERT_EQ(action_state_->polygon_name, "Stop"); - // 4. Restoring back normal operation - publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); + // 5. Restoring back normal operation + publishScan(4.5, curr_time); + ASSERT_TRUE(waitData(4.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index db3e80805f..4b4f42ec55 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -53,6 +53,8 @@ static const std::vector ARBITRARY_POLYGON { static const double CIRCLE_RADIUS{0.5}; static const int MIN_POINTS{2}; static const double SLOWDOWN_RATIO{0.7}; +static const double LINEAR_LIMIT{0.4}; +static const double ANGULAR_LIMIT{0.09}; static const double TIME_BEFORE_COLLISION{1.0}; static const double SIMULATION_TIME_STEP{0.01}; static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; @@ -276,6 +278,16 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st test_node_->set_parameter( rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO)); + test_node_->declare_parameter( + polygon_name + ".linear_limit", rclcpp::ParameterValue(LINEAR_LIMIT)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".linear_limit", LINEAR_LIMIT)); + + test_node_->declare_parameter( + polygon_name + ".angular_limit", rclcpp::ParameterValue(ANGULAR_LIMIT)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".angular_limit", ANGULAR_LIMIT)); + test_node_->declare_parameter( polygon_name + ".time_before_collision", rclcpp::ParameterValue(TIME_BEFORE_COLLISION)); @@ -462,6 +474,20 @@ TEST_F(Tester, testPolygonGetSlowdownParameters) EXPECT_NEAR(polygon_->getSlowdownRatio(), SLOWDOWN_RATIO, EPSILON); } +TEST_F(Tester, testPolygonGetLimitParameters) +{ + createPolygon("limit", true); + + // Check that common parameters set correctly + EXPECT_EQ(polygon_->getName(), POLYGON_NAME); + EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::LIMIT); + EXPECT_EQ(polygon_->getMinPoints(), MIN_POINTS); + EXPECT_EQ(polygon_->isVisualize(), true); + // Check that limit params are correct + EXPECT_NEAR(polygon_->getLinearLimit(), LINEAR_LIMIT, EPSILON); + EXPECT_NEAR(polygon_->getAngularLimit(), ANGULAR_LIMIT, EPSILON); +} + TEST_F(Tester, testPolygonGetApproachParameters) { createPolygon("approach", true);