diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 05f92364fb7..5edbeb3cc9a 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -119,6 +119,12 @@ class Polygon * @return Time before collision in seconds */ double getTimeBeforeCollision() const; + /** + * @brief Obtains time step for robot movement simulation for current polygon. + * Applicable for APPROACH model. + * @return Simulation time step in seconds + */ + double getSimulationTimeStep() const; /** * @brief Obtains minimum velocity before completly stopping. diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index 3943e8f0bf6..f4df29d1ef7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -70,7 +70,7 @@ class VelocityPolygon : public Polygon protected: /** - * @brief Custom struc to store the parameters of the sub-polygon + * @brief Custom struct to store the parameters of the sub-polygon * @param poly_ The points of the sub-polygon * @param velocity_polygon_name_ The name of the sub-polygon * @param linear_min_ The minimum linear velocity @@ -79,6 +79,12 @@ class VelocityPolygon : public Polygon * @param theta_max_ The maximum angular velocity * @param direction_end_angle_ The end angle of the direction(For holonomic robot only) * @param direction_start_angle_ The start angle of the direction(For holonomic robot only) + * @param slowdown_ratio_ Robot slowdown (share of its actual speed) + * @param linear_limit_ Robot linear limit + * @param angular_limit_ Robot angular limit + * @param time_before_collision_ Time before collision in seconds + * @param simulation_time_step_ Time step for robot movement simulation + * @param min_vel_before_stop_ Minimum velocity before we fully stop */ struct SubPolygonParameter { @@ -90,6 +96,12 @@ class VelocityPolygon : public Polygon double theta_max_; double direction_end_angle_; double direction_start_angle_; + double slowdown_ratio_; + double linear_limit_; + double angular_limit_; + double time_before_collision_; + double simulation_time_step_; + double min_vel_before_stop_; }; /** diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 8daddf1ddfb..51bca24aa8f 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -173,6 +173,11 @@ double Polygon::getTimeBeforeCollision() const return time_before_collision_; } +double Polygon::getSimulationTimeStep() const +{ + return simulation_time_step_; +} + double Polygon::getMinVelBeforeStop() const { return min_vel_before_stop_; diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index eeb5082aab7..dd3fc6fd145 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -123,9 +123,56 @@ bool VelocityPolygon::getParameters( .as_double(); } + double slowdown_ratio = 0.0; + if (action_type_ == SLOWDOWN) { + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".slowdown_ratio", rclcpp::ParameterValue( + 0.5)); + slowdown_ratio = node->get_parameter( + polygon_name_ + "." + velocity_polygon_name + ".slowdown_ratio").as_double(); + } + + double linear_limit = 0.0; + double angular_limit = 0.0; + if (action_type_ == LIMIT) { + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".linear_limit", rclcpp::ParameterValue( + 0.5)); + linear_limit = node->get_parameter( + polygon_name_ + "." + velocity_polygon_name + ".linear_limit").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".angular_limit", rclcpp::ParameterValue( + 0.5)); + angular_limit = node->get_parameter( + polygon_name_ + "." + velocity_polygon_name + ".angular_limit").as_double(); + } + + double time_before_collision = 0.0; + double simulation_time_step = 0.0; + double min_vel_before_stop = 0.0; + if (action_type_ == APPROACH) { + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".time_before_collision", rclcpp::ParameterValue( + 2.0)); + time_before_collision = node->get_parameter( + polygon_name_ + "." + velocity_polygon_name + ".time_before_collision").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".simulation_time_step", rclcpp::ParameterValue( + 0.1)); + simulation_time_step = node->get_parameter( + polygon_name_ + "." + velocity_polygon_name + ".simulation_time_step").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".min_vel_before_stop", rclcpp::ParameterValue( + -1.0)); + min_vel_before_stop = node->get_parameter( + polygon_name_ + "." + velocity_polygon_name + ".min_vel_before_stop").as_double(); + } + SubPolygonParameter sub_polygon = { poly, velocity_polygon_name, linear_min, linear_max, theta_min, - theta_max, direction_end_angle, direction_start_angle}; + theta_max, direction_end_angle, direction_start_angle, + slowdown_ratio, linear_limit, angular_limit, time_before_collision, simulation_time_step, + min_vel_before_stop}; sub_polygons_.push_back(sub_polygon); } } catch (const std::exception & ex) { @@ -153,6 +200,14 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in) // p_s.z will remain 0.0 polygon_.polygon.points.push_back(p_s); } + + slowdown_ratio_ = sub_polygon.slowdown_ratio_; + linear_limit_ = sub_polygon.linear_limit_; + angular_limit_ = sub_polygon.angular_limit_; + time_before_collision_ = sub_polygon.time_before_collision_; + simulation_time_step_ = sub_polygon.simulation_time_step_; + min_vel_before_stop_ = sub_polygon.min_vel_before_stop_; + return; } } diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index e1efc8bd015..43701574503 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -67,6 +67,13 @@ static const char RIGHT_POLYGON_STR[]{ static const bool IS_HOLONOMIC{true}; static const bool IS_NOT_HOLONOMIC{false}; static const int MIN_POINTS{2}; +static const double SLOWDOWN_RATIO{0.25}; +static const double LINEAR_LIMIT{0.3}; +static const double ANGULAR_LIMIT{0.2}; +static const double TIME_BEFORE_COLLISION{2.0}; +static const double SIMULATION_TIME_STEP{0.05}; +static const double MIN_VEL_BEFORE_COLLISION{-0.5}; + static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; class TestNode : public nav2_util::LifecycleNode @@ -145,6 +152,10 @@ class Tester : public ::testing::Test protected: // Working with parameters void setCommonParameters(const std::string & polygon_name, const std::string & action_type); + void addSlowdownParameters(const std::string & polygon_name); + void addLimitParameters(const std::string & polygon_name); + void addApproachParameters(const std::string & polygon_name); + void setVelocityPolygonParameters(const bool is_holonomic); void addPolygonVelocitySubPolygon( const std::string & sub_polygon_name, @@ -211,6 +222,39 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC)); } +void Tester::addSlowdownParameters(const std::string & polygon_name) +{ + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + "." + polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO)); +} + +void Tester::addLimitParameters(const std::string & polygon_name) +{ + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + "." + polygon_name + ".linear_limit", LINEAR_LIMIT)); + + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + "." + polygon_name + ".angular_limit", ANGULAR_LIMIT)); +} + +void Tester::addApproachParameters(const std::string & polygon_name) +{ + test_node_->set_parameter( + rclcpp::Parameter( + std::string( + POLYGON_NAME) + "." + polygon_name + ".time_before_collision", TIME_BEFORE_COLLISION)); + + test_node_->set_parameter( + rclcpp::Parameter( + std::string(POLYGON_NAME) + "." + polygon_name + ".simulation_time_step", + SIMULATION_TIME_STEP)); + + test_node_->set_parameter( + rclcpp::Parameter( + std::string(POLYGON_NAME) + "." + polygon_name + ".min_vel_before_stop", + MIN_VEL_BEFORE_COLLISION)); +} + void Tester::setVelocityPolygonParameters(const bool is_holonomic) { test_node_->declare_parameter( @@ -270,6 +314,7 @@ void Tester::setVelocityPolygonParameters(const bool is_holonomic) rclcpp::Parameter(std::string(POLYGON_NAME) + ".velocity_polygons", velocity_polygons)); } + void Tester::addPolygonVelocitySubPolygon( const std::string & sub_polygon_name, const double linear_min, const double linear_max, @@ -560,6 +605,35 @@ TEST_F(Tester, testVelocityPolygonHolonomicVelocitySwitching) EXPECT_NEAR(poly[3].y, RIGHT_POLYGON[7], EPSILON); } +// TEST_F(Tester, testVelocityPolygonSlowdownParameters) +// { +// createVelocityPolygon("slowdown", IS_NOT_HOLONOMIC); +// addSlowdownParameters(SUB_POLYGON_FORWARD_NAME); + +// EXPECT_EQ(velocity_polygon_->getActionType(), nav2_collision_monitor::SLOWDOWN); +// EXPECT_NEAR(velocity_polygon_->getSlowdownRatio(), 0.25, EPSILON); +// } + +// TEST_F(Tester, testVelocityPolygonLimitParameters) +// { +// createVelocityPolygon("limit", IS_NOT_HOLONOMIC); +// addLimitParameters(SUB_POLYGON_FORWARD_NAME); + +// EXPECT_EQ(velocity_polygon_->getActionType(), nav2_collision_monitor::LIMIT); +// EXPECT_NEAR(velocity_polygon_->getLinearLimit(), 0.3, EPSILON); +// EXPECT_NEAR(velocity_polygon_->getAngularLimit(), 0.2, EPSILON); +// } + +// TEST_F(Tester, testVelocityPolygonApproachParameters) +// { +// createVelocityPolygon("approach", IS_NOT_HOLONOMIC); +// addApproachParameters(SUB_POLYGON_FORWARD_NAME); + +// EXPECT_EQ(velocity_polygon_->getActionType(), nav2_collision_monitor::APPROACH); +// EXPECT_NEAR(velocity_polygon_->getTimeBeforeCollision(), 2.0, EPSILON); +// EXPECT_NEAR(velocity_polygon_->getSimulationTimeStep(), 0.05, EPSILON); +// EXPECT_NEAR(velocity_polygon_->getMinVelBeforeStop(), -0.5, EPSILON); +// } int main(int argc, char ** argv) {