diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 09a13658092..6cee8c22f9d 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -33,6 +33,7 @@ #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" #include "nav2_collision_monitor/circle.hpp" +#include "nav2_collision_monitor/velocity_polygon.hpp" #include "nav2_collision_monitor/source.hpp" #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index f02759815bc..5e342cdb7f1 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -28,7 +28,6 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_costmap_2d/footprint_subscriber.hpp" -#include "nav2_collision_monitor/velocity_polygon.hpp" #include "nav2_collision_monitor/types.hpp" namespace nav2_collision_monitor @@ -133,15 +132,10 @@ class Polygon */ virtual bool isShapeSet(); - /** - * @brief Returns true if using velocity based polygon - */ - bool isUsingVelocityPolygonSelector(); - /** * @brief Updates polygon from footprint subscriber (if any) */ - void updatePolygon(const Velocity & cmd_vel_in); + virtual void updatePolygon(const Velocity & /*cmd_vel_in*/); /** * @brief Gets number of points inside given polygon @@ -168,14 +162,6 @@ class Polygon */ void publish(); - /** - * @brief Set the polygon shape based on the polygon points - * @param poly_points Polygon points in the format of x1, y1, x2, y2 ... - * @param poly Output polygon points (vertices) - * @return True if successfully set the polygon shape - */ - static bool setPolygonShape(std::vector & poly_points, std::vector & poly); - protected: /** * @brief Supporting routine obtaining ROS-parameters common for all shapes @@ -281,9 +267,6 @@ class Polygon /// @brief Polygon publisher for visualization purposes rclcpp_lifecycle::LifecyclePublisher::SharedPtr polygon_pub_; - /// @brief Velocity polygon (if any) - std::vector> velocity_polygons_; - /// @brief Polygon points (vertices) in a base_frame_id_ std::vector poly_; }; // class Polygon 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 f8ccee6d138..6c10dfa31ea 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Dexory +// Copyright (c) 2023 Dexory // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,12 +18,12 @@ #include #include -#include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" -#include "tf2_ros/buffer.h" - -#include "nav2_util/lifecycle_node.hpp" +#include "nav2_collision_monitor/polygon.hpp" #include "nav2_collision_monitor/types.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" namespace nav2_collision_monitor { @@ -33,21 +33,17 @@ namespace nav2_collision_monitor * This class contains all the points of the polygon and * the expected condition of the velocity based polygon. */ -class VelocityPolygon +class VelocityPolygon : public Polygon { public: /** * @brief VelocityPolygon constructor * @param node Collision Monitor node pointer * @param polygon_name Name of main polygon - * @param velocity_polygon_name Name of velocity polygon */ VelocityPolygon( - const nav2_util::LifecycleNode::WeakPtr & node, - const std::string & polygon_name, - const std::string & velocity_polygon_name, - const std::shared_ptr tf_buffer, - const std::string & base_frame_id, + const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name, + const std::shared_ptr tf_buffer, const std::string & base_frame_id, const tf2::Duration & transform_tolerance); /** * @brief VelocityPolygon destructor @@ -58,73 +54,39 @@ class VelocityPolygon * @brief Supporting routine obtaining velocity polygon specific ROS-parameters * @return True if all parameters were obtained or false in failure case */ - bool getParameters(); - - /** - * @brief Check if the velocities and direction is in expected range. - * @param cmd_vel_in Robot twist command input - * @return True if speed and direction is within the condition - */ - bool isInRange(const Velocity & cmd_vel_in); - - /** - * @brief Check if the velocities and direction is in expected range. - * @param cmd_vel_in Robot twist command input - * @return True if speed and direction is within the condition - */ - std::vector getPolygon(); + bool getParameters( + std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic, + std::string & /*footprint_topic*/) override; protected: - // ----- Variables ----- + // override the base class update polygon + void updatePolygon(const Velocity & cmd_vel_in) override; - /// @brief Collision Monitor node - nav2_util::LifecycleNode::WeakPtr node_; - /// @brief Collision monitor node logger stored for further usage - rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + bool holonomic_; - /** - * @brief Dynamic polygon callback - * @param msg Shared pointer to the polygon message - */ - void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg); + // Define a structure to store the basic parameters + struct SubPolygonParameter + { + std::vector poly_; + std::string velocity_polygon_name_; + double linear_min_; + double linear_max_; + double theta_min_; + double theta_max_; + double direction_end_angle_; + double direction_start_angle_; + }; + + // Create a vector to store instances of BasicParameters + std::vector sub_polygons_; /** - * @brief Updates polygon from geometry_msgs::msg::PolygonStamped message - * @param msg Message to update polygon from + * @brief Check if the velocities and direction is in expected range. + * @param cmd_vel_in Robot twist command input + * @return True if speed and direction is within the condition */ - void updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg); - - // Basic parameters - /// @brief Points of the polygon - std::vector poly_; - /// @brief Name of polygon - std::string polygon_name_; - /// @brief Polygon subscription - rclcpp::Subscription::SharedPtr polygon_sub_; - /// @brief velocity_polygon_name Name of velocity polygon - std::string velocity_polygon_name_; - /// @brief Holonomic flag (true for holonomic, false for non-holonomic) - bool holonomic_; - /// @brief Maximum twist linear velocity - double linear_max_; - /// @brief Minimum twist linear velocity - double linear_min_; - /// @brief End angle of velocity direction for holonomic model - double direction_end_angle_; - /// @brief Start angle of velocity direction for holonomic model - double direction_start_angle_; - /// @brief Maximum twist rotational speed - double theta_max_; - /// @brief Minimum twist rotational speed - double theta_min_; + bool isInRange(const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon_param); - // Global variables - /// @brief TF buffer - std::shared_ptr tf_buffer_; - /// @brief Base frame ID - std::string base_frame_id_; - /// @brief Transform tolerance - tf2::Duration transform_tolerance_; }; // class VelocityPolygon } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index db36fbe1c6b..7f2ceafa6af 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -14,7 +14,8 @@ collision_monitor: # Footprint could be "polygon" type with dynamically set footprint from footprint_topic # or "circle" type with static footprint set by radius. "footprint_topic" parameter # to be ignored in circular case. - polygons: ["PolygonStop" , "FootprintApproach"] + # or "velocity_polygon" type with dynamically set polygon from velocity_polygons + polygons: ["PolygonStop"] PolygonStop: type: "polygon" points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" @@ -45,38 +46,43 @@ collision_monitor: FootprintApproach: type: "polygon" action_type: "approach" - # footprint_topic: "/local_costmap/published_footprint" + footprint_topic: "/local_costmap/published_footprint" time_before_collision: 2.0 simulation_time_step: 0.1 min_points: 6 visualize: False enabled: True + VelocityPolygonApproach: + type: "velocity_polygon" + action_type: "approach" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: True + enabled: True polygon_pub_topic: "polygon_approach" - velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stop"] # third priority + velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stop"] + holonomic: false rotation: - points: [0.3, 0.3, 0.3, -0.3, -0.3, -0.3, -0.3, 0.3] - holonomic: false + points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]" linear_min: 0.0 linear_max: 0.05 theta_min: -1.0 theta_max: 1.0 translation_forward: - points: [0.35, 0.3, 0.35, -0.3, -0.2, -0.3, -0.2, 0.3] - holonomic: false + points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]" linear_min: 0.0 linear_max: 1.0 theta_min: -1.0 theta_max: 1.0 translation_backward: - points: [0.2, 0.3, 0.2, -0.3, -0.35, -0.3, -0.35, 0.3] - holonomic: false + points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]" linear_min: -1.0 linear_max: 0.0 theta_min: -1.0 theta_max: 1.0 stop: - points: [0.25, 0.25, 0.25, -0.25, -0.25, -0.25, -0.25, 0.25] - holonomic: false + points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]" linear_min: -1.0 linear_max: 1.0 theta_min: -1.0 diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 2078341ae09..d88e85604e9 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -298,6 +298,10 @@ bool CollisionMonitor::configurePolygons( polygons_.push_back( std::make_shared( node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + } else if (polygon_type == "velocity_polygon") { + polygons_.push_back( + std::make_shared( + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); } else { // Error if something else RCLCPP_ERROR( get_logger(), diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 2566aea7991..502b187844d 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -14,7 +14,6 @@ #include "nav2_collision_monitor/polygon.hpp" -#include #include #include @@ -180,46 +179,8 @@ bool Polygon::isShapeSet() return true; } -bool Polygon::isUsingVelocityPolygonSelector() +void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/) { - return !velocity_polygons_.empty(); -} - -void Polygon::updatePolygon(const Velocity & cmd_vel_in) -{ - if (isUsingVelocityPolygonSelector()) { - for (auto & velocity_polygon : velocity_polygons_) { - - if (velocity_polygon->isInRange(cmd_vel_in)) { - // Set the polygon that is within the speed range - poly_ = velocity_polygon->getPolygon(); - - // Update visualization polygon - polygon_.polygon.points.clear(); - 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); - } - - return; - } - } - - // Log for uncovered velocity - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - RCLCPP_WARN_THROTTLE( - logger_, - *node->get_clock(), 2.0, "Velocity is not covered by any of the velocity polygons. x: %.3f y: %.3f tw: %.3f ", - cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw); - return; - } - if (footprint_sub_ != nullptr) { // Get latest robot footprint from footprint subscriber std::vector footprint_vec; @@ -456,48 +417,22 @@ bool Polygon::getParameters( polygon_name_.c_str()); } - try { - 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(); - return true; - } 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::PARAMETER_STRING); - footprint_topic = - node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); - return true; - } - } catch (const rclcpp::exceptions::ParameterUninitializedException &) { - RCLCPP_INFO( - logger_, - "[%s]: Polygon topic are not defined. Using velocity polygon instead.", - polygon_name_.c_str()); - } - - - // Velocity polygon will be used - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".velocity_polygons", rclcpp::PARAMETER_STRING_ARRAY); - std::vector velocity_polygons = node->get_parameter( - polygon_name_ + ".velocity_polygons").as_string_array(); - - for (std::string velocity_polygon_name : velocity_polygons) { - auto velocity_polygon = std::make_shared( - node_, polygon_name_, velocity_polygon_name, tf_buffer_, base_frame_id_, - transform_tolerance_); - if (!velocity_polygon->getParameters()) { - return false; - } - velocity_polygons_.emplace_back(velocity_polygon); + 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(); } - } catch (const std::exception & ex) { RCLCPP_ERROR( logger_, @@ -648,26 +583,4 @@ bool Polygon::getPolygonFromString( return true; } -bool Polygon::setPolygonShape(std::vector & poly_points, std::vector & poly) -{ - // Check for points format correctness - if (poly_points.size() <= 6 || poly_points.size() % 2 != 0) { - return false; - } - - // Obtain polygon vertices - Point point; - bool first = true; - for (double val : poly_points) { - if (first) { - point.x = val; - } else { - point.y = val; - poly.push_back(point); - } - first = !first; - } - return true; -} - } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 7927138bbe1..e0526e4c51e 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Dexory +// Copyright (c) 2023 Dexory // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,204 +13,188 @@ // limitations under the License. #include "nav2_collision_monitor/velocity_polygon.hpp" -#include "nav2_collision_monitor/polygon.hpp" #include "nav2_util/node_utils.hpp" - namespace nav2_collision_monitor { VelocityPolygon::VelocityPolygon( - const nav2_util::LifecycleNode::WeakPtr & node, - const std::string & polygon_name, - const std::string & velocity_polygon_name, - const std::shared_ptr tf_buffer, - const std::string & base_frame_id, + const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name, + const std::shared_ptr tf_buffer, const std::string & base_frame_id, const tf2::Duration & transform_tolerance) -: node_(node), polygon_name_(polygon_name), velocity_polygon_name_(velocity_polygon_name), - tf_buffer_(tf_buffer), base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance) +: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) { - RCLCPP_INFO(logger_, "[%s]: Creating VelocityPolygon", velocity_polygon_name_.c_str()); + RCLCPP_INFO(logger_, "[%s]: Creating VelocityPolygon", polygon_name_.c_str()); } VelocityPolygon::~VelocityPolygon() { - RCLCPP_INFO(logger_, "[%s]: Destroying VelocityPolygon", velocity_polygon_name_.c_str()); - poly_.clear(); + RCLCPP_INFO(logger_, "[%s]: Destroying VelocityPolygon", polygon_name_.c_str()); } -bool VelocityPolygon::getParameters() +bool VelocityPolygon::getParameters( + std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic, + std::string & /*footprint_topic*/) { auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; } - 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_ + "." + velocity_polygon_name_ + ".points", - rclcpp::PARAMETER_DOUBLE_ARRAY); - std::vector polygon_points = node->get_parameter( - polygon_name_ + "." + velocity_polygon_name_ + ".points").as_double_array(); - - if (!Polygon::setPolygonShape(polygon_points, poly_)) { - RCLCPP_ERROR( - logger_, - "[%s][%s]: Polygon has incorrect points description", - polygon_name_.c_str(), velocity_polygon_name_.c_str()); - return false; - } - } catch (const rclcpp::exceptions::ParameterUninitializedException &) { - RCLCPP_INFO( - logger_, - "[%s][%s]: Polygon points are not defined. Will wait for dynamic subscription instead.", - polygon_name_.c_str(), velocity_polygon_name_.c_str()); - } + if (!getCommonParameters(polygon_pub_topic)) { + return false; + } - std::string polygon_sub_topic = "/collision_monitor/" + polygon_name_ + "/" + - velocity_polygon_name_ + "/set_polygon"; - rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default - polygon_sub_ = node->create_subscription( - polygon_sub_topic, polygon_qos, - std::bind(&VelocityPolygon::polygonCallback, this, std::placeholders::_1)); + try { + // Get velocity_polygons parameter + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".velocity_polygons", rclcpp::PARAMETER_STRING_ARRAY); + std::vector velocity_polygons = + node->get_parameter(polygon_name_ + ".velocity_polygons").as_string_array(); // holonomic param nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + "." + velocity_polygon_name_ + ".holonomic", - rclcpp::ParameterValue(false)); - holonomic_ = node->get_parameter( - polygon_name_ + "." + velocity_polygon_name_ + ".holonomic").as_bool(); + node, polygon_name_ + ".holonomic", rclcpp::ParameterValue(false)); + holonomic_ = node->get_parameter(polygon_name_ + ".holonomic").as_bool(); - // linear_max param - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + "." + velocity_polygon_name_ + ".linear_max", - rclcpp::ParameterValue(0.0)); - linear_max_ = node->get_parameter( - polygon_name_ + "." + velocity_polygon_name_ + ".linear_max").as_double(); + for (std::string velocity_polygon_name : velocity_polygons) { + // polygon points parameter + std::vector poly; + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".points", rclcpp::PARAMETER_STRING); + std::string poly_string = + node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".points").as_string(); - // linear_min param - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + "." + velocity_polygon_name_ + ".linear_min", - rclcpp::ParameterValue(0.0)); - linear_min_ = node->get_parameter( - polygon_name_ + "." + velocity_polygon_name_ + ".linear_min").as_double(); + if (!getPolygonFromString(poly_string, poly)) { + return false; + } - // direction_end_angle param - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + "." + velocity_polygon_name_ + ".direction_end_angle", - rclcpp::ParameterValue(0.0)); - direction_end_angle_ = node->get_parameter( - polygon_name_ + "." + velocity_polygon_name_ + ".direction_end_angle").as_double(); + // linear_min param + double linear_min; + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".linear_min", + rclcpp::ParameterValue(0.0)); + linear_min = node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".linear_min") + .as_double(); - // direction_start_angle param - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + "." + velocity_polygon_name_ + ".direction_start_angle", - rclcpp::ParameterValue(0.0)); - direction_start_angle_ = node->get_parameter( - polygon_name_ + "." + velocity_polygon_name_ + ".direction_start_angle").as_double(); + // linear_max param + double linear_max; + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".linear_max", + rclcpp::ParameterValue(0.0)); + linear_max = node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".linear_max") + .as_double(); - // theta_max param - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + "." + velocity_polygon_name_ + ".theta_max", rclcpp::ParameterValue( - 0.0)); - theta_max_ = node->get_parameter( - polygon_name_ + "." + velocity_polygon_name_ + ".theta_max").as_double(); + // theta_min param + double theta_min; + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".theta_min", + rclcpp::ParameterValue(0.0)); + theta_min = + node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".theta_min").as_double(); - // theta_min param - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + "." + velocity_polygon_name_ + ".theta_min", rclcpp::ParameterValue( - 0.0)); - theta_min_ = node->get_parameter( - polygon_name_ + "." + velocity_polygon_name_ + ".theta_min").as_double(); + // theta_max param + double theta_max; + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".theta_max", + rclcpp::ParameterValue(0.0)); + theta_max = + node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".theta_max").as_double(); + + // direction_end_angle param and direction_start_angle param + double direction_end_angle = 0.0; + double direction_start_angle = 0.0; + if (holonomic_) { + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".direction_end_angle", + rclcpp::ParameterValue(0.0)); + direction_end_angle = + node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".direction_end_angle") + .as_double(); + + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + "." + velocity_polygon_name + ".direction_start_angle", + rclcpp::ParameterValue(0.0)); + direction_start_angle = + node + ->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".direction_start_angle") + .as_double(); + } + SubPolygonParameter sub_polygon = { + poly, velocity_polygon_name, linear_min, linear_max, theta_min, + theta_max, direction_end_angle, direction_start_angle}; + sub_polygons_.emplace_back(sub_polygon); + } } catch (const std::exception & ex) { RCLCPP_ERROR( - logger_, - "[%s][%s]: Error while getting polygon parameters: %s", - polygon_name_.c_str(), velocity_polygon_name_.c_str(), ex.what()); + logger_, "[%s]: Error while getting polygon parameters: %s", polygon_name_.c_str(), + ex.what()); return false; } - return true; } -void VelocityPolygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) +void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in) { - RCLCPP_INFO( - logger_, - "[%s][%s]: Polygon shape update has been arrived", - polygon_name_.c_str(), velocity_polygon_name_.c_str()); - updatePolygon(msg); -} - -void VelocityPolygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) -{ - std::size_t new_size = msg->polygon.points.size(); - - if (new_size < 3) { - RCLCPP_ERROR(logger_, "[%s]: Polygon should have at least 3 points", polygon_name_.c_str()); - return; - } - - // Get the transform from PolygonStamped frame to base_frame_id_ - tf2::Transform tf_transform; - if ( - !nav2_util::getTransform( - msg->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; + for (auto & sub_polygon : sub_polygons_) { + if (isInRange(cmd_vel_in, sub_polygon)) { + // Set the polygon that is within the speed range + poly_ = sub_polygon.poly_; + + // Update visualization polygon + polygon_.polygon.points.clear(); + 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); + } + return; + } } - // Set main poly_ vertices first time - poly_.resize(new_size); - for (std::size_t i = 0; i < new_size; i++) { - // Transform point coordinates from PolygonStamped frame -> to base frame - tf2::Vector3 p_v3_s(msg->polygon.points[i].x, msg->polygon.points[i].y, 0.0); - tf2::Vector3 p_v3_b = tf_transform * p_v3_s; - - // Fill poly_ array - poly_[i] = {p_v3_b.x(), p_v3_b.y()}; + // Log for uncovered velocity + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; } + RCLCPP_WARN_THROTTLE( + logger_, *node->get_clock(), 2.0, + "Velocity is not covered by any of the velocity polygons. x: %.3f y: %.3f tw: %.3f ", + cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw); + return; } -bool VelocityPolygon::isInRange(const Velocity & cmd_vel_in) -{ +bool VelocityPolygon::isInRange( + const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon) { if (holonomic_) { const double twist_linear = std::hypot(cmd_vel_in.x, cmd_vel_in.y); // check if direction in angle range(min -> max) double direction = std::atan2(cmd_vel_in.y, cmd_vel_in.x); bool direction_in_range; - if (direction_start_angle_ <= direction_end_angle_) { - direction_in_range = (direction >= direction_start_angle_ && - direction <= direction_end_angle_); + if (sub_polygon.direction_start_angle_ <= sub_polygon.direction_end_angle_) { + direction_in_range = + (direction >= sub_polygon.direction_start_angle_ && + direction <= sub_polygon.direction_end_angle_); } else { - direction_in_range = (direction >= direction_start_angle_ || - direction <= direction_end_angle_); + direction_in_range = + (direction >= sub_polygon.direction_start_angle_ || + direction <= sub_polygon.direction_end_angle_); } - return twist_linear <= linear_max_ && - twist_linear >= linear_min_ && - direction_in_range && - cmd_vel_in.tw <= theta_max_ && - cmd_vel_in.tw >= theta_min_; + return twist_linear <= sub_polygon.linear_max_ && twist_linear >= sub_polygon.linear_min_ && + direction_in_range && cmd_vel_in.tw <= sub_polygon.theta_max_ && + cmd_vel_in.tw >= sub_polygon.theta_min_; } else { // non-holonomic - return cmd_vel_in.x <= linear_max_ && - cmd_vel_in.x >= linear_min_ && - cmd_vel_in.tw <= theta_max_ && - cmd_vel_in.tw >= theta_min_; + return cmd_vel_in.x <= sub_polygon.linear_max_ && cmd_vel_in.x >= sub_polygon.linear_min_ && + cmd_vel_in.tw <= sub_polygon.theta_max_ && cmd_vel_in.tw >= sub_polygon.theta_min_; } -} - -std::vector VelocityPolygon::getPolygon() -{ - return poly_; + return true; } } // namespace nav2_collision_monitor