diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 8cef0f609f..d812df7d65 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -32,6 +33,7 @@ set(dependencies rclcpp_components sensor_msgs geometry_msgs + std_msgs tf2 tf2_ros tf2_geometry_msgs diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index c440032701..a468be7c88 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -19,6 +19,8 @@ #include #include +#include "std_msgs/msg/float32.hpp" + #include "nav2_collision_monitor/polygon.hpp" namespace nav2_collision_monitor @@ -67,15 +69,16 @@ 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: /** * @brief Supporting routine obtaining polygon-specific ROS-parameters - * @brief polygon_sub_topic Input name of polygon subscription topic - * @param polygon_pub_topic Output name of polygon publishing topic + * @param polygon_sub_topic Input name of polygon subscription topic + * @param polygon_pub_topic Output name of polygon or radius publishing topic * @param footprint_topic Output name of footprint topic. For Circle returns empty string, * there is no footprint subscription in this class. * @return True if all parameters were obtained or false in failure case @@ -85,12 +88,34 @@ class Circle : public Polygon std::string & polygon_pub_topic, std::string & footprint_topic) override; + /** + * @brief Creates polygon or radius topic subscription + * @param polygon_sub_topic Output name of polygon or radius subscription topic. + * Empty, if no polygon subscription. + */ + void createSubscription(std::string & polygon_sub_topic) override; + + /** + * @brief Updates polygon from radius value + * @param radius New circle radius to update polygon + */ + void updatePolygon(double radius); + + /** + * @brief Dynamic circle radius callback + * @param msg Shared pointer to the radius value message + */ + void radiusCallback(std_msgs::msg::Float32::ConstSharedPtr msg); + + // ----- Variables ----- /// @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 } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 1e6a72d159..fdc9e07b7a 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(); @@ -165,14 +165,24 @@ class Polygon protected: /** * @brief Supporting routine obtaining ROS-parameters common for all shapes - * @param polygon_pub_topic Output name of polygon publishing topic + * @param polygon_pub_topic Output name of polygon or radius subscription topic. + * Empty, if no polygon subscription. + * @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_pub_topic); + bool getCommonParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic, + bool use_dynamic_sub = false); /** * @brief Supporting routine obtaining polygon-specific ROS-parameters - * @brief polygon_sub_topic Output name of polygon subscription topic. + * @param polygon_sub_topic Output name of polygon or radius subscription topic. * Empty, if no polygon subscription. * @param polygon_pub_topic Output name of polygon publishing topic * @param footprint_topic Output name of footprint topic. @@ -184,6 +194,13 @@ class Polygon std::string & polygon_pub_topic, std::string & footprint_topic); + /** + * @brief Creates polygon or radius topic subscription + * @param polygon_sub_topic Output name of polygon or radius subscription topic. + * Empty, if no polygon subscription. + */ + virtual void createSubscription(std::string & polygon_sub_topic); + /** * @brief Updates polygon from geometry_msgs::msg::PolygonStamped message * @param msg Message to update polygon from diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 4889ea77f1..fd663526e8 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -17,6 +17,7 @@ tf2_geometry_msgs sensor_msgs geometry_msgs + std_msgs nav2_common nav2_util nav2_costmap_2d diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index fa0fc33a4f..353db72dc0 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, @@ -80,29 +89,91 @@ bool Circle::getParameters( throw std::runtime_error{"Failed to lock node"}; } - if (!getCommonParameters(polygon_pub_topic)) { - return false; - } - - // There is no polygon or footprint subscription for the Circle. Thus, set strings as empty. + // Clear the polygon subscription topic. It will be set later, if necessary. polygon_sub_topic.clear(); - 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_; - } catch (const std::exception & ex) { - RCLCPP_ERROR( + use_dynamic_sub = false; + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { + RCLCPP_INFO( logger_, - "[%s]: Error while getting circle parameters: %s", - polygon_name_.c_str(), ex.what()); - return false; + "[%s]: Polygon circle radius is not defined. Using dynamic subscription instead.", + polygon_name_.c_str()); } - return true; + 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; + } + + // 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) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!polygon_sub_topic.empty()) { + RCLCPP_INFO( + logger_, + "[%s]: Subscribing on %s topic for polygon", + polygon_name_.c_str(), polygon_sub_topic.c_str()); + rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + if (polygon_subscribe_transient_local_) { + polygon_qos.transient_local(); + } + radius_sub_ = node->create_subscription( + polygon_sub_topic, polygon_qos, + std::bind(&Circle::radiusCallback, this, std::placeholders::_1)); + } +} + +void Circle::updatePolygon(double radius) +{ + // Update circle radius + radius_ = radius; + radius_squared_ = radius_ * radius_; + + // Create a polygon from radius and store it + std::vector poly; + getPolygon(poly); + polygon_.polygon.points.clear(); // clear polygon points + for (const Point & p : poly) { + geometry_msgs::msg::Point32 p_s; + p_s.x = p.x; + p_s.y = p.y; + // p_s.z will remain 0.0 + polygon_.polygon.points.push_back(p_s); // add new points + } +} + +void Circle::radiusCallback(std_msgs::msg::Float32::ConstSharedPtr msg) +{ + RCLCPP_INFO( + logger_, + "[%s]: Polygon circle radius update has been arrived", + polygon_name_.c_str()); + updatePolygon(msg->data); } } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 6a0801d1d2..7e0a69b84f 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -65,19 +65,7 @@ bool Polygon::configure() return false; } - if (!polygon_sub_topic.empty()) { - RCLCPP_INFO( - logger_, - "[%s]: Subscribing on %s topic for polygon", - polygon_name_.c_str(), polygon_sub_topic.c_str()); - rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default - if (polygon_subscribe_transient_local_) { - polygon_qos.transient_local(); - } - polygon_sub_ = node->create_subscription( - polygon_sub_topic, polygon_qos, - std::bind(&Polygon::polygonCallback, this, std::placeholders::_1)); - } + createSubscription(polygon_sub_topic); if (!footprint_topic.empty()) { RCLCPP_INFO( @@ -287,7 +275,11 @@ void Polygon::publish() polygon_pub_->publish(std::move(msg)); } -bool Polygon::getCommonParameters(std::string & polygon_pub_topic) +bool Polygon::getCommonParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic, + bool use_dynamic_sub_topic) { auto node = node_.lock(); if (!node) { @@ -372,6 +364,28 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) node, polygon_name_ + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name_)); polygon_pub_topic = node->get_parameter(polygon_name_ + ".polygon_pub_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_, @@ -393,63 +407,64 @@ bool Polygon::getParameters( throw std::runtime_error{"Failed to lock node"}; } - if (!getCommonParameters(polygon_pub_topic)) { - return false; - } - // Clear the subscription topics. They will be set later, if necessary. polygon_sub_topic.clear(); footprint_topic.clear(); + bool use_dynamic_sub = true; // if getting parameter points fails, use dynamic subscription try { - try { - // Leave it uninitialized: it will throw an inner exception if the parameter is not set - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING); - 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. - return getPolygonFromString(poly_string, poly_); - } catch (const rclcpp::exceptions::ParameterUninitializedException &) { - RCLCPP_INFO( + // Leave it uninitialized: it will throw an inner exception if the parameter is not set + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING); + std::string poly_string = + node->get_parameter(polygon_name_ + ".points").as_string(); + + use_dynamic_sub = !getPolygonFromString(poly_string, poly_); + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { + RCLCPP_INFO( + logger_, + "[%s]: Polygon points are not defined. Using dynamic subscription instead.", + 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]: Polygon points are not defined. Using dynamic subscription instead.", + "[%s]: Error while getting polygon parameters:" + " static points and sub topic both not defined", polygon_name_.c_str()); } - - if (action_type_ == STOP || action_type_ == SLOWDOWN || action_type_ == LIMIT || - action_type_ == DO_NOTHING) - { - // Dynamic polygon will be used - 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 if (action_type_ == APPROACH) { - // 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(); - } catch (const std::exception & ex) { - RCLCPP_ERROR( - logger_, - "[%s]: Error while getting polygon parameters: %s", - polygon_name_.c_str(), ex.what()); return false; } return true; } +void Polygon::createSubscription(std::string & polygon_sub_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!polygon_sub_topic.empty()) { + RCLCPP_INFO( + logger_, + "[%s]: Subscribing on %s topic for polygon", + polygon_name_.c_str(), polygon_sub_topic.c_str()); + rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + if (polygon_subscribe_transient_local_) { + polygon_qos.transient_local(); + } + polygon_sub_ = node->create_subscription( + polygon_sub_topic, polygon_qos, + std::bind(&Polygon::polygonCallback, this, std::placeholders::_1)); + } +} + void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) { std::size_t new_size = msg->polygon.points.size(); @@ -582,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 84779a1a2d..602a018c2b 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -34,8 +34,9 @@ VelocityPolygon::~VelocityPolygon() } bool VelocityPolygon::getParameters( - std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic, - std::string & /*footprint_topic*/) + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic) { auto node = node_.lock(); if (!node) { @@ -43,7 +44,7 @@ bool VelocityPolygon::getParameters( } clock_ = node->get_clock(); - if (!getCommonParameters(polygon_pub_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 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