diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index 552d0429479..a468be7c880 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -69,9 +69,10 @@ class Circle : public Polygon int getPointsInside(const std::vector & points) const override; /** - * @brief Specifies that the shape is always set for a circle object + * @brief Returns true if circle radius is set. + * Otherwise, prints a warning and returns false. */ - bool isShapeSet() override {return true;} + bool isShapeSet() override; protected: /** @@ -112,7 +113,7 @@ class Circle : public Polygon /// @brief Radius of the circle double radius_; /// @brief (radius * radius) value. Stored for optimization. - double radius_squared_; + double radius_squared_ = -1.0; /// @brief Radius subscription rclcpp::Subscription::SharedPtr radius_sub_; }; // class Circle diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index b0dc066a187..fdc9e07b7a3 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -128,7 +128,7 @@ class Polygon /** * @brief Returns true if polygon points were set. - * Othewise, prints a warning and returns false. + * Otherwise, prints a warning and returns false. */ virtual bool isShapeSet(); @@ -170,12 +170,15 @@ class Polygon * @param polygon_sub_topic Output name of polygon publishing topic * @param footprint_topic Output name of footprint topic. * Empty, if no footprint subscription. + * @param use_dynamic_sub If false, the parameter polygon_sub_topic or footprint_topic + * will not be declared * @return True if all parameters were obtained or false in failure case */ bool getCommonParameters( std::string & polygon_sub_topic, std::string & polygon_pub_topic, - std::string & footprint_topic); + std::string & footprint_topic, + bool use_dynamic_sub = false); /** * @brief Supporting routine obtaining polygon-specific ROS-parameters diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index 598e0c8af12..353db72dc07 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -70,6 +70,15 @@ int Circle::getPointsInside(const std::vector & points) const return num; } +bool Circle::isShapeSet() +{ + if (radius_squared_ == -1.0) { + RCLCPP_WARN(logger_, "[%s]: Circle radius is not set yet", polygon_name_.c_str()); + return false; + } + return true; +} + bool Circle::getParameters( std::string & polygon_sub_topic, std::string & polygon_pub_topic, @@ -83,24 +92,14 @@ bool Circle::getParameters( // Clear the polygon subscription topic. It will be set later, if necessary. polygon_sub_topic.clear(); - if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) { - return false; - } - - // There is no footprint subscription for the Circle. Thus, set string as empty. - footprint_topic.clear(); - + bool use_dynamic_sub = true; // if getting parameter radius fails, use dynamic subscription try { // Leave it not initialized: the will cause an error if it will not set nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".radius", rclcpp::PARAMETER_DOUBLE); radius_ = node->get_parameter(polygon_name_ + ".radius").as_double(); radius_squared_ = radius_ * radius_; - - // Do not need to proceed further, if "radius" parameter is defined. - // Static polygon will be used. - polygon_sub_topic.clear(); - return true; + use_dynamic_sub = false; } catch (const rclcpp::exceptions::ParameterUninitializedException &) { RCLCPP_INFO( logger_, @@ -108,15 +107,23 @@ bool Circle::getParameters( polygon_name_.c_str()); } - if (polygon_sub_topic.empty()) { - RCLCPP_ERROR( - logger_, - "[%s]: Error while getting circle parameters: static radius and sub topic both not defined", - polygon_name_.c_str()); - return false; + bool ret = true; + if (!getCommonParameters( + polygon_sub_topic, polygon_pub_topic, footprint_topic, use_dynamic_sub)) + { + if (use_dynamic_sub && polygon_sub_topic.empty()) { + RCLCPP_ERROR( + logger_, + "[%s]: Error while getting circle parameters: static radius and sub topic both not defined", + polygon_name_.c_str()); + } + ret = false; } - return true; + // There is no footprint subscription for the Circle. Thus, set string as empty. + footprint_topic.clear(); + + return ret; } void Circle::createSubscription(std::string & polygon_sub_topic) diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 5ca3c84bfc8..7e0a69b84f4 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -278,7 +278,8 @@ void Polygon::publish() bool Polygon::getCommonParameters( std::string & polygon_sub_topic, std::string & polygon_pub_topic, - std::string & footprint_topic) + std::string & footprint_topic, + bool use_dynamic_sub_topic) { auto node = node_.lock(); if (!node) { @@ -364,25 +365,27 @@ bool Polygon::getCommonParameters( polygon_pub_topic = node->get_parameter(polygon_name_ + ".polygon_pub_topic").as_string(); } - if (action_type_ != APPROACH) { - // Get polygon sub topic, in case dynamic polygon will be used - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".polygon_sub_topic", rclcpp::ParameterValue("")); - polygon_sub_topic = - node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string(); - } else { - // Obtain the footprint topic to make a footprint subscription for approach polygon - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".footprint_topic", - rclcpp::ParameterValue("local_costmap/published_footprint")); - footprint_topic = - node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); - } - nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".polygon_subscribe_transient_local", rclcpp::ParameterValue(false)); polygon_subscribe_transient_local_ = node->get_parameter(polygon_name_ + ".polygon_subscribe_transient_local").as_bool(); + + if (use_dynamic_sub_topic) { + if (action_type_ != APPROACH) { + // Get polygon sub topic + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING); + polygon_sub_topic = + node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string(); + } else { + // Obtain the footprint topic to make a footprint subscription for approach polygon + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".footprint_topic", + rclcpp::ParameterValue("local_costmap/published_footprint")); + footprint_topic = + node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); + } + } } catch (const std::exception & ex) { RCLCPP_ERROR( logger_, @@ -408,10 +411,7 @@ bool Polygon::getParameters( polygon_sub_topic.clear(); footprint_topic.clear(); - if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) { - return false; - } - + bool use_dynamic_sub = true; // if getting parameter points fails, use dynamic subscription try { // Leave it uninitialized: it will throw an inner exception if the parameter is not set nav2_util::declare_parameter_if_not_declared( @@ -419,11 +419,7 @@ bool Polygon::getParameters( std::string poly_string = node->get_parameter(polygon_name_ + ".points").as_string(); - // Do not need to proceed further, if "points" parameter is defined. - // Static polygon will be used. - polygon_sub_topic.clear(); - footprint_topic.clear(); - return getPolygonFromString(poly_string, poly_); + use_dynamic_sub = !getPolygonFromString(poly_string, poly_); } catch (const rclcpp::exceptions::ParameterUninitializedException &) { RCLCPP_INFO( logger_, @@ -431,11 +427,16 @@ bool Polygon::getParameters( polygon_name_.c_str()); } - if (polygon_sub_topic.empty() && footprint_topic.empty()) { - RCLCPP_ERROR( - logger_, - "[%s]: Error while getting polygon parameters: static points and sub topic both not defined", - polygon_name_.c_str()); + if (!getCommonParameters( + polygon_sub_topic, polygon_pub_topic, footprint_topic, use_dynamic_sub)) + { + if (use_dynamic_sub && polygon_sub_topic.empty() && footprint_topic.empty()) { + RCLCPP_ERROR( + logger_, + "[%s]: Error while getting polygon parameters:" + " static points and sub topic both not defined", + polygon_name_.c_str()); + } return false; } @@ -596,6 +597,7 @@ bool Polygon::getPolygonFromString( "Points in the polygon specification must be pairs of numbers" "Found a point with %d numbers.", static_cast(vvf[i].size())); + polygon.clear(); return false; } } diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 9c0c6cda42f..602a018c2bb 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -44,7 +44,7 @@ bool VelocityPolygon::getParameters( } clock_ = node->get_clock(); - if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) { + if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic, false)) { return false; } diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 849b6278686..c703e12557b 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -117,6 +117,17 @@ class TestNode : public nav2_util::LifecycleNode polygon_pub_->publish(std::move(msg)); } + void publishRadius() + { + radius_pub_ = this->create_publisher( + POLYGON_SUB_TOPIC, rclcpp::SystemDefaultsQoS()); + + std::unique_ptr msg = std::make_unique(); + msg->data = CIRCLE_RADIUS; + + radius_pub_->publish(std::move(msg)); + } + void publishFootprint() { footprint_pub_ = this->create_publisher( @@ -159,6 +170,7 @@ class TestNode : public nav2_util::LifecycleNode private: rclcpp::Publisher::SharedPtr polygon_pub_; + rclcpp::Publisher::SharedPtr radius_pub_; rclcpp::Publisher::SharedPtr footprint_pub_; rclcpp::Subscription::SharedPtr polygon_sub_; @@ -227,11 +239,11 @@ class Tester : public ::testing::Test void setPolygonParameters( const char * points, const bool is_static); - void setCircleParameters(const double radius); + void setCircleParameters(const double radius, const bool is_static); bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param); // Creating routines void createPolygon(const std::string & action_type, const bool is_static); - void createCircle(const std::string & action_type); + void createCircle(const std::string & action_type, const bool is_static); void sendTransforms(double shift); @@ -240,6 +252,9 @@ class Tester : public ::testing::Test const std::chrono::nanoseconds & timeout, std::vector & poly); + // Wait until circle polygon radius will be received + bool waitRadius(const std::chrono::nanoseconds & timeout); + // Wait until footprint will be received bool waitFootprint( const std::chrono::nanoseconds & timeout, @@ -344,12 +359,19 @@ void Tester::setPolygonParameters( } } -void Tester::setCircleParameters(const double radius) +void Tester::setCircleParameters(const double radius, const bool is_static) { - test_node_->declare_parameter( - std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius)); + if (is_static) { + test_node_->declare_parameter( + std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius)); + } else { + test_node_->declare_parameter( + std::string(CIRCLE_NAME) + ".polygon_sub_topic", rclcpp::ParameterValue(POLYGON_SUB_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".polygon_sub_topic", POLYGON_SUB_TOPIC)); + } } bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const std::string & param) @@ -382,10 +404,10 @@ void Tester::createPolygon(const std::string & action_type, const bool is_static polygon_->activate(); } -void Tester::createCircle(const std::string & action_type) +void Tester::createCircle(const std::string & action_type, const bool is_static) { setCommonParameters(CIRCLE_NAME, action_type); - setCircleParameters(CIRCLE_RADIUS); + setCircleParameters(CIRCLE_RADIUS, is_static); circle_ = std::make_shared( test_node_, CIRCLE_NAME, @@ -433,6 +455,19 @@ bool Tester::waitPolygon( return false; } +bool Tester::waitRadius(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (circle_->isShapeSet()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + bool Tester::waitFootprint( const std::chrono::nanoseconds & timeout, std::vector & footprint) @@ -518,7 +553,7 @@ TEST_F(Tester, testPolygonGetApproachParameters) TEST_F(Tester, testCircleGetParameters) { - createCircle("approach"); + createCircle("approach", true); // Check that common parameters set correctly EXPECT_EQ(circle_->getName(), CIRCLE_NAME); @@ -628,8 +663,9 @@ TEST_F(Tester, testCircleUndeclaredRadius) tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_FALSE(circle_->configure()); - // Check that "radius" parameter is not set after configuring + // Check that "radius" and "polygon_sub_topic" parameters are not set after configuring ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "radius")); + ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "polygon_sub_topic")); } TEST_F(Tester, testPolygonTopicUpdate) @@ -646,7 +682,7 @@ TEST_F(Tester, testPolygonTopicUpdate) ASSERT_FALSE(waitPolygon(100ms, poly)); ASSERT_FALSE(polygon_->isShapeSet()); - // Publush correct polygon and make shure that it was set correctly + // Publish correct polygon and make sure that it was set correctly test_node_->publishPolygon(BASE_FRAME_ID, true); ASSERT_TRUE(waitPolygon(500ms, poly)); ASSERT_TRUE(polygon_->isShapeSet()); @@ -661,6 +697,18 @@ TEST_F(Tester, testPolygonTopicUpdate) EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7], EPSILON); } +TEST_F(Tester, testCircleTopicUpdate) +{ + createCircle("stop", false); + ASSERT_FALSE(circle_->isShapeSet()); + + // Publish radius and make sure that it was set correctly + test_node_->publishRadius(); + ASSERT_TRUE(waitRadius(500ms)); + EXPECT_NEAR(circle_->getRadius(), CIRCLE_RADIUS, EPSILON); + EXPECT_NEAR(circle_->getRadiusSquared(), CIRCLE_RADIUS * CIRCLE_RADIUS, EPSILON); +} + TEST_F(Tester, testPolygonTopicUpdateDifferentFrame) { createPolygon("stop", false); @@ -789,7 +837,7 @@ TEST_F(Tester, testPolygonGetPointsInsideEdge) TEST_F(Tester, testCircleGetPointsInside) { - createCircle("stop"); + createCircle("stop", true); std::vector points; // Point out of radius