diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index 552d042947..a468be7c88 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 b0dc066a18..bfaebfc729 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(); diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index 598e0c8af1..016f925a61 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 radius_defined = false; 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; + radius_defined = true; } catch (const rclcpp::exceptions::ParameterUninitializedException &) { RCLCPP_INFO( logger_, @@ -108,15 +107,21 @@ 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)) { + if (!radius_defined && 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 5ca3c84bfc..3a32c5c06f 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -364,25 +364,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 (!isShapeSet()) { + // If shape is not already set, dynamic subscription will be used + if (action_type_ != APPROACH) { + // Get polygon sub topic + nav2_util::declare_parameter_if_not_declared(node, polygon_name_ + ".polygon_sub_topic"); + 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 +410,7 @@ bool Polygon::getParameters( polygon_sub_topic.clear(); footprint_topic.clear(); - if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) { - return false; - } - + bool points_defined = false; 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 +418,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_); + points_defined = getPolygonFromString(poly_string, poly_); } catch (const rclcpp::exceptions::ParameterUninitializedException &) { RCLCPP_INFO( logger_, @@ -431,14 +426,18 @@ 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)) { + if (!points_defined && 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; } + return true; } diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 849b627868..c703e12557 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