diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index c440032701..eae86a82f2 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -74,8 +74,8 @@ class Circle : public Polygon 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 +85,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_; + /// @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..b0dc066a18 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -165,14 +165,21 @@ 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. * @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); /** * @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 +191,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/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index fa0fc33a4f..a18158dcb4 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -80,12 +80,14 @@ bool Circle::getParameters( throw std::runtime_error{"Failed to lock node"}; } - if (!getCommonParameters(polygon_pub_topic)) { + // 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 polygon or footprint subscription for the Circle. Thus, set strings as empty. - polygon_sub_topic.clear(); + // There is no footprint subscription for the Circle. Thus, set string as empty. footprint_topic.clear(); try { @@ -94,15 +96,77 @@ bool Circle::getParameters( 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) { + + // Do not need to proceed further, if "radius" parameter is defined. + // Static polygon will be used. + polygon_sub_topic.clear(); + return true; + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { + RCLCPP_INFO( + logger_, + "[%s]: Polygon circle radius is not defined. Using dynamic subscription instead.", + polygon_name_.c_str()); + } + + if (polygon_sub_topic.empty()) { RCLCPP_ERROR( logger_, - "[%s]: Error while getting circle parameters: %s", - polygon_name_.c_str(), ex.what()); + "[%s]: Error while getting polygon parameters: static radius and sub topic both not defined", + polygon_name_.c_str()); return false; } return true; } +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..5ca3c84bfc 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,10 @@ 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) { auto node = node_.lock(); if (!node) { @@ -372,6 +363,26 @@ 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(); } + + 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(); } catch (const std::exception & ex) { RCLCPP_ERROR( logger_, @@ -393,63 +404,66 @@ 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(); - 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( - 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)) { + return false; + } - 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(); - } + 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_ + ".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) { + 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. + polygon_sub_topic.clear(); + footprint_topic.clear(); + return 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 (polygon_sub_topic.empty() && footprint_topic.empty()) { RCLCPP_ERROR( logger_, - "[%s]: Error while getting polygon parameters: %s", - polygon_name_.c_str(), ex.what()); + "[%s]: Error while getting polygon parameters: static points and sub topic both not defined", + polygon_name_.c_str()); 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(); diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 84779a1a2d..9c0c6cda42 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)) { return false; }