diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 8cef0f609fc..b902cc4ea83 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -49,7 +49,6 @@ set(detector_library_name ${detector_executable_name}_core) add_library(${monitor_library_name} SHARED src/collision_monitor_node.cpp src/polygon.cpp - src/velocity_polygon.cpp src/circle.cpp src/source.cpp src/scan.cpp @@ -60,7 +59,6 @@ add_library(${monitor_library_name} SHARED add_library(${detector_library_name} SHARED src/collision_detector_node.cpp src/polygon.cpp - src/velocity_polygon.cpp src/circle.cpp src/source.cpp src/scan.cpp diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index dff60b5c84d..1c5b32dc7d1 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -36,8 +36,6 @@ The zones around the robot can take the following shapes: * Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface. * Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time. * Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape. -* VelocityPolygon: allow switching of polygons based on the command velocity. When the velocity is covered by multiple sub polygons, the first sub polygon in the `velocity_polygons` list will be used. This is useful for robots to set different safety zones based on their velocity (e.g. a robot that has a larger safety zone when moving at 1.0 m/s than when moving at 0.5 m/s). - The data may be obtained from different data sources: @@ -50,13 +48,8 @@ The data may be obtained from different data sources: The Collision Monitor is designed to operate below Nav2 as an independent safety node. This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate. -The following diagram is showing the high-level design of Collision Monitor module. All shapes (`Polygon`, `Circle` and `VelocityPolygon`) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model. +The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model. ![HLD.png](doc/HLD.png) - -`VelocityPolygon` can be configured with multiple sub polygons and can switch between them based on the velocity. -![dexory_velocity_polygon.gif](doc/dexory_velocity_polygon.gif) - - ### Configuration Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages. diff --git a/nav2_collision_monitor/doc/dexory_velocity_polygon.gif b/nav2_collision_monitor/doc/dexory_velocity_polygon.gif deleted file mode 100644 index 4fe2e1ad5ce..00000000000 Binary files a/nav2_collision_monitor/doc/dexory_velocity_polygon.gif and /dev/null differ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index fae479d6504..7791265179f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -33,7 +33,6 @@ #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/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 6cee8c22f9d..09a13658092 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,7 +33,6 @@ #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 62a8d653816..940c53148ac 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -135,7 +135,7 @@ class Polygon /** * @brief Updates polygon from footprint subscriber (if any) */ - virtual void updatePolygon(const Velocity & /*cmd_vel_in*/); + void updatePolygon(); /** * @brief Gets number of points inside given 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 deleted file mode 100644 index e4c65bebad5..00000000000 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ /dev/null @@ -1,115 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_ -#define NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_ - -#include -#include -#include - -#include "geometry_msgs/msg/polygon_stamped.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 -{ - -/** - * @brief Velocity polygon class. - * This class contains all the points of the polygon and - * the expected condition of the velocity based polygon. - */ -class VelocityPolygon : public Polygon -{ -public: - /** - * @brief VelocityPolygon constructor - * @param node Collision Monitor node pointer - * @param polygon_name Name of main polygon - */ - VelocityPolygon( - 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 - */ - virtual ~VelocityPolygon(); - - /** - * @brief Overriden getParameters function for VelocityPolygon parameters - * @param polygon_sub_topic Not used in VelocityPolygon - * @param polygon_pub_topic Output name of polygon publishing topic - * @param footprint_topic Not used in VelocityPolygon - * @return True if all parameters were obtained or false in failure case - */ - bool getParameters( - std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic, - std::string & /*footprint_topic*/) override; - - /** - * @brief Overriden updatePolygon function for VelocityPolygon - * @param cmd_vel_in Robot twist command input - */ - void updatePolygon(const Velocity & cmd_vel_in) override; - -protected: - /** - * @brief Custom struc to store the parameters of the sub-polygon - * @param poly_ The points of the sub-polygon - * @param velocity_polygon_name_ The name of the sub-polygon - * @param linear_min_ The minimum linear velocity - * @param linear_max_ The maximum linear velocity - * @param theta_min_ The minimum angular velocity - * @param theta_max_ The maximum angular velocity - * @param direction_end_angle_ The end angle of the direction(For holonomic robot only) - * @param direction_start_angle_ The start angle of the direction(For holonomic robot only) - */ - 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_; - }; - - /** - * @brief Check if the velocities and direction is in expected range. - * @param cmd_vel_in Robot twist command input - * @param sub_polygon_param Sub polygon parameters - * @return True if speed and direction is within the condition - */ - bool isInRange(const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon_param); - - // Clock - rclcpp::Clock::SharedPtr clock_; - - // Variables - /// @brief Flag to indicate if the robot is holonomic - bool holonomic_; - /// @brief Vector to store the parameters of the sub-polygon - std::vector sub_polygons_; -}; // class VelocityPolygon - -} // namespace nav2_collision_monitor - -#endif // NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_ diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 74db6c3e8a7..3e506ed1d7f 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -11,10 +11,9 @@ collision_monitor: stop_pub_timeout: 2.0 # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, # and robot footprint for "approach" action type. - # (1) Footprint could be "polygon" type with dynamically set footprint from footprint_topic - # (2) "circle" type with static footprint set by radius. "footprint_topic" parameter + # 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. - # (3) "velocity_polygon" type with dynamically set polygon from velocity_polygons polygons: ["PolygonStop"] PolygonStop: type: "polygon" @@ -52,42 +51,6 @@ collision_monitor: min_points: 6 visualize: False enabled: True - VelocityPolygonStop: - type: "velocity_polygon" - action_type: "stop" - min_points: 6 - visualize: True - enabled: True - polygon_pub_topic: "velocity_polygon_stop" - velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"] - holonomic: false - rotation: - 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]]" - 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]]" - linear_min: -1.0 - linear_max: 0.0 - theta_min: -1.0 - theta_max: 1.0 - # This is the last polygon to be checked, it should cover the entire range of robot's velocities - # It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity - # is not covered by any of the other sub-polygons - stopped: - 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 - theta_max: 1.0 observation_sources: ["scan"] scan: type: "scan" diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 39224a03fbf..d5cf99a9de7 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -207,10 +207,6 @@ bool CollisionDetector::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/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 6709e2f0393..115a6ffef88 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -297,10 +297,6 @@ 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(), @@ -444,7 +440,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) } // Update polygon coordinates - polygon->updatePolygon(cmd_vel_in); + polygon->updatePolygon(); const ActionType at = polygon->getActionType(); if (at == STOP || at == SLOWDOWN || at == LIMIT) { diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 38950f4a31b..c215cdf2da8 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -181,7 +181,7 @@ bool Polygon::isShapeSet() return true; } -void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/) +void Polygon::updatePolygon() { if (footprint_sub_ != nullptr) { // Get latest robot footprint from footprint subscriber diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp deleted file mode 100644 index 84779a1a2d4..00000000000 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "nav2_collision_monitor/velocity_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::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_tolerance) -: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) -{ - RCLCPP_INFO(logger_, "[%s]: Creating VelocityPolygon", polygon_name_.c_str()); -} - -VelocityPolygon::~VelocityPolygon() -{ - RCLCPP_INFO(logger_, "[%s]: Destroying VelocityPolygon", polygon_name_.c_str()); -} - -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"}; - } - clock_ = node->get_clock(); - - if (!getCommonParameters(polygon_pub_topic)) { - return false; - } - - 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_ + ".holonomic", rclcpp::ParameterValue(false)); - holonomic_ = node->get_parameter(polygon_name_ + ".holonomic").as_bool(); - - 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(); - - if (!getPolygonFromString(poly_string, poly)) { - return false; - } - - // linear_min param - double linear_min; - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + "." + velocity_polygon_name + ".linear_min", - rclcpp::PARAMETER_DOUBLE); - linear_min = node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".linear_min") - .as_double(); - - // linear_max param - double linear_max; - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + "." + velocity_polygon_name + ".linear_max", - rclcpp::PARAMETER_DOUBLE); - linear_max = node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".linear_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::PARAMETER_DOUBLE); - 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::PARAMETER_DOUBLE); - 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(M_PI)); - 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(-M_PI)); - 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_.push_back(sub_polygon); - } - } 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 VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in) -{ - 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; - } - } - - // Log for uncovered velocity - RCLCPP_WARN_THROTTLE( - logger_, *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, const SubPolygonParameter & sub_polygon) -{ - bool in_range = - (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_); - - if (holonomic_) { - // Additionally check if moving direction in angle range(start -> end) for holonomic case - const double direction = std::atan2(cmd_vel_in.y, cmd_vel_in.x); - if (sub_polygon.direction_start_angle_ <= sub_polygon.direction_end_angle_) { - in_range &= - (direction >= sub_polygon.direction_start_angle_ && - direction <= sub_polygon.direction_end_angle_); - } else { - in_range &= - (direction >= sub_polygon.direction_start_angle_ || - direction <= sub_polygon.direction_end_angle_); - } - } - - return in_range; -} - -} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt index 2f879e1d396..f2b4619bbd7 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -25,15 +25,6 @@ target_link_libraries(polygons_test ${monitor_library_name} ) -# Velocity Polygon test -ament_add_gtest(velocity_polygons_test velocity_polygons_test.cpp) -ament_target_dependencies(velocity_polygons_test - ${dependencies} -) -target_link_libraries(velocity_polygons_test - ${monitor_library_name} -) - # Collision Monitor node test ament_add_gtest(collision_monitor_node_test collision_monitor_node_test.cpp) ament_target_dependencies(collision_monitor_node_test diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index cd78eb1a378..2ceb34fff6a 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -68,8 +68,7 @@ enum PolygonType { POLYGON_UNKNOWN = 0, POLYGON = 1, - CIRCLE = 2, - VELOCITY_POLYGON = 3 + CIRCLE = 2 }; enum SourceType @@ -142,18 +141,10 @@ class Tester : public ::testing::Test void addPolygon( const std::string & polygon_name, const PolygonType type, const double size, const std::string & at); - void addPolygonVelocitySubPolygon( - const std::string & polygon_name, const std::string & sub_polygon_name, - const double linear_min, const double linear_max, - const double theta_min, const double theta_max, - const double size); void addSource(const std::string & source_name, const SourceType type); void setVectors( const std::vector & polygons, const std::vector & sources); - void setPolygonVelocityVectors( - const std::string & polygon_name, - const std::vector & polygons); // Setting TF chains void sendTransforms(const rclcpp::Time & stamp); @@ -332,15 +323,6 @@ void Tester::addPolygon( polygon_name + ".radius", rclcpp::ParameterValue(size)); cm_->set_parameter( rclcpp::Parameter(polygon_name + ".radius", size)); - } else if (type == VELOCITY_POLYGON) { - cm_->declare_parameter( - polygon_name + ".type", rclcpp::ParameterValue("velocity_polygon")); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + ".type", "velocity_polygon")); - cm_->declare_parameter( - polygon_name + ".holonomic", rclcpp::ParameterValue(false)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + ".holonomic", false)); } else { // type == POLYGON_UNKNOWN cm_->declare_parameter( polygon_name + ".type", rclcpp::ParameterValue("unknown")); @@ -399,43 +381,6 @@ void Tester::addPolygon( rclcpp::Parameter(polygon_name + ".polygon_pub_topic", polygon_name)); } -void Tester::addPolygonVelocitySubPolygon( - const std::string & polygon_name, const std::string & sub_polygon_name, - const double linear_min, const double linear_max, - const double theta_min, const double theta_max, - const double size) -{ - const std::string points = "[[" + - std::to_string(size) + ", " + std::to_string(size) + "], [" + - std::to_string(size) + ", " + std::to_string(-size) + "], [" + - std::to_string(-size) + ", " + std::to_string(-size) + "], [" + - std::to_string(-size) + ", " + std::to_string(size) + "]]"; - cm_->declare_parameter( - polygon_name + "." + sub_polygon_name + ".points", rclcpp::ParameterValue(points)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + "." + sub_polygon_name + ".points", points)); - - cm_->declare_parameter( - polygon_name + "." + sub_polygon_name + ".linear_min", rclcpp::ParameterValue(linear_min)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + "." + sub_polygon_name + ".linear_min", linear_min)); - - cm_->declare_parameter( - polygon_name + "." + sub_polygon_name + ".linear_max", rclcpp::ParameterValue(linear_max)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + "." + sub_polygon_name + ".linear_max", linear_max)); - - cm_->declare_parameter( - polygon_name + "." + sub_polygon_name + ".theta_min", rclcpp::ParameterValue(theta_min)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + "." + sub_polygon_name + ".theta_min", theta_min)); - - cm_->declare_parameter( - polygon_name + "." + sub_polygon_name + ".theta_max", rclcpp::ParameterValue(theta_max)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + "." + sub_polygon_name + ".theta_max", theta_max)); -} - void Tester::addSource( const std::string & source_name, const SourceType type) { @@ -492,14 +437,6 @@ void Tester::setVectors( cm_->set_parameter(rclcpp::Parameter("observation_sources", sources)); } -void Tester::setPolygonVelocityVectors( - const std::string & polygon_name, - const std::vector & polygons) -{ - cm_->declare_parameter(polygon_name + ".velocity_polygons", rclcpp::ParameterValue(polygons)); - cm_->set_parameter(rclcpp::Parameter(polygon_name + ".velocity_polygons", polygons)); -} - void Tester::sendTransforms(const rclcpp::Time & stamp) { std::shared_ptr tf_broadcaster = @@ -1293,74 +1230,6 @@ TEST_F(Tester, testCollisionPointsMarkers) cm_->stop(); } -TEST_F(Tester, testVelocityPolygonStop) -{ - // Set Collision Monitor parameters. - // Add velocity polygon with 2 sub polygon: - // 1. Forward: 0 -> 0.5 m/s - // 2. Backward: 0 -> -0.5 m/s - setCommonParameters(); - addPolygon("VelocityPoylgon", VELOCITY_POLYGON, 1.0, "stop"); - addPolygonVelocitySubPolygon("VelocityPoylgon", "Forward", 0.0, 0.5, 0.0, 1.0, 4.0); - addPolygonVelocitySubPolygon("VelocityPoylgon", "Backward", -0.5, 0.0, 0.0, 1.0, 2.0); - setPolygonVelocityVectors("VelocityPoylgon", {"Forward", "Backward"}); - addSource(POINTCLOUD_NAME, POINTCLOUD); - setVectors({"VelocityPoylgon"}, {POINTCLOUD_NAME}); - - rclcpp::Time curr_time = cm_->now(); - // Start Collision Monitor node - cm_->start(); - // Check that robot stops when source is enabled - sendTransforms(curr_time); - - // 1. Obstacle is far away from Forward velocity polygon - publishPointCloud(4.5, curr_time); - ASSERT_TRUE(waitData(std::hypot(4.5, 0.01), 500ms, curr_time)); - publishCmdVel(0.4, 0.0, 0.1); - ASSERT_TRUE(waitCmdVel(500ms)); - ASSERT_NEAR(cmd_vel_out_->linear.x, 0.4, EPSILON); - ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); - ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); - - // 2. Obstacle is in Forward velocity polygon - publishPointCloud(3.0, curr_time); - ASSERT_TRUE(waitData(std::hypot(3.0, 0.01), 500ms, curr_time)); - publishCmdVel(0.4, 0.0, 0.1); - ASSERT_TRUE(waitCmdVel(500ms)); - ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); - ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); - ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); - ASSERT_TRUE(waitActionState(500ms)); - ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "VelocityPoylgon"); - - // 3. Switch to Backward velocity polygon - // Obstacle is far away from Backward velocity polygon - publishCmdVel(-0.4, 0.0, 0.1); - ASSERT_TRUE(waitCmdVel(500ms)); - ASSERT_NEAR(cmd_vel_out_->linear.x, -0.4, EPSILON); - ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); - ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); - ASSERT_TRUE(waitActionState(500ms)); - ASSERT_EQ(action_state_->action_type, DO_NOTHING); - ASSERT_EQ(action_state_->polygon_name, ""); - - // 4. Obstacle is in Backward velocity polygon - publishPointCloud(-1.5, curr_time); - ASSERT_TRUE(waitData(std::hypot(-1.5, 0.01), 500ms, curr_time)); - publishCmdVel(-0.4, 0.0, 0.1); - ASSERT_TRUE(waitCmdVel(500ms)); - ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); - ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); - ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); - ASSERT_TRUE(waitActionState(500ms)); - ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "VelocityPoylgon"); - - // Stop Collision Monitor node - cm_->stop(); -} - int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index c616544469c..2ab46cdcede 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -425,9 +425,8 @@ bool Tester::waitFootprint( std::vector & footprint) { rclcpp::Time start_time = test_node_->now(); - nav2_collision_monitor::Velocity vel{0.0, 0.0, 0.0}; while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { - polygon_->updatePolygon(vel); + polygon_->updatePolygon(); polygon_->getPolygon(footprint); if (footprint.size() > 0) { return true; @@ -674,9 +673,8 @@ TEST_F(Tester, testPolygonTopicUpdateDifferentFrame) // Move BASE2_FRAME_ID to 0.2 m away from BASE_FRAME_ID sendTransforms(0.2); - // updatePolygon(vel) should update poly coordinates to correct ones in BASE_FRAME_ID - nav2_collision_monitor::Velocity vel{0.0, 0.0, 0.0}; - polygon_->updatePolygon(vel); + // updatePolygon() should update poly coordinates to correct ones in BASE_FRAME_ID + polygon_->updatePolygon(); // Check that polygon coordinates were updated correctly ASSERT_TRUE(waitPolygon(500ms, poly)); ASSERT_EQ(poly.size(), 4u); diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp deleted file mode 100644 index 7030ddd2b20..00000000000 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ /dev/null @@ -1,577 +0,0 @@ -// Copyright (c) 2024 Dexory -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "geometry_msgs/msg/point32.hpp" -#include "geometry_msgs/msg/polygon_stamped.hpp" - -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" - -#include "nav2_collision_monitor/types.hpp" -#include "nav2_collision_monitor/polygon.hpp" -#include "nav2_collision_monitor/velocity_polygon.hpp" - -using namespace std::chrono_literals; - -static constexpr double EPSILON = std::numeric_limits::epsilon(); - -static const char BASE_FRAME_ID[]{"base_link"}; -static const char BASE2_FRAME_ID[]{"base2_link"}; -static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; -static const char POLYGON_NAME[]{"TestVelocityPolygon"}; -static const char SUB_POLYGON_FORWARD_NAME[]{"Forward"}; -static const char SUB_POLYGON_BACKWARD_NAME[]{"Backward"}; -static const char SUB_POLYGON_LEFT_NAME[]{"Left"}; -static const char SUB_POLYGON_RIGHT_NAME[]{"Right"}; -static const std::vector FORWARD_POLYGON{ - 0.5, 0.5, 0.5, -0.5, 0.0, -0.5, 0.0, 0.5}; -static const std::vector BACKWARD_POLYGON{ - 0.0, 0.5, 0.0, -0.5, -0.5, -0.5, -0.5, 0.5}; -static const std::vector LEFT_POLYGON{ - 0.5, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0, -0.5}; -static const std::vector RIGHT_POLYGON{ - 0.5, 0.0, 0.5, -0.5, -0.5, -0.5, 0.0, 0.0}; -static const char FORWARD_POLYGON_STR[]{ - "[[0.5, 0.5], [0.5, -0.5], [0.0, -0.5], [0.0, 0.5]]"}; -static const char BACKWARD_POLYGON_STR[]{ - "[[0.0, 0.5], [0.0, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"}; -static const char LEFT_POLYGON_STR[]{ - "[[0.5, 0.5], [0.5, 0.0], [0.0, 0.0], [0.0, -0.5]]"}; -static const char RIGHT_POLYGON_STR[]{ - "[[0.5, 0.0], [0.5, -0.5], [-0.5, -0.5], [0.0, 0.0]]"}; - -static const bool IS_HOLONOMIC{true}; -static const bool IS_NOT_HOLONOMIC{false}; -static const int MIN_POINTS{2}; -static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; - -class TestNode : public nav2_util::LifecycleNode -{ -public: - TestNode() - : nav2_util::LifecycleNode("test_node"), polygon_received_(nullptr) - { - polygon_sub_ = this->create_subscription( - POLYGON_PUB_TOPIC, rclcpp::SystemDefaultsQoS(), - std::bind(&TestNode::polygonCallback, this, std::placeholders::_1)); - } - - ~TestNode() {} - - void polygonCallback(geometry_msgs::msg::PolygonStamped::SharedPtr msg) - { - polygon_received_ = msg; - } - - geometry_msgs::msg::PolygonStamped::SharedPtr waitPolygonReceived( - const std::chrono::nanoseconds & timeout) - { - rclcpp::Time start_time = this->now(); - while (rclcpp::ok() && this->now() - start_time <= rclcpp::Duration(timeout)) { - if (polygon_received_) { - return polygon_received_; - } - rclcpp::spin_some(this->get_node_base_interface()); - std::this_thread::sleep_for(10ms); - } - return nullptr; - } - -private: - rclcpp::Subscription::SharedPtr polygon_sub_; - geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received_; -}; // TestNode - -class VelocityPolygonWrapper : public nav2_collision_monitor::VelocityPolygon -{ -public: - VelocityPolygonWrapper( - 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) - : nav2_collision_monitor::VelocityPolygon( - node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) - { - } - - double isHolonomic() const - { - return holonomic_; - } - - double isVisualize() const - { - return visualize_; - } - - std::vector getSubPolygons() - { - return sub_polygons_; - } -}; // VelocityPolygonWrapper - -class Tester : public ::testing::Test -{ -public: - Tester(); - ~Tester(); - -protected: - // Working with parameters - void setCommonParameters(const std::string & polygon_name, const std::string & action_type); - void setVelocityPolygonParameters(const bool is_holonomic); - void addPolygonVelocitySubPolygon( - const std::string & sub_polygon_name, - const double linear_min, const double linear_max, - const double theta_min, const double theta_max, - const double direction_end_angle, const double direction_start_angle, - const std::string & polygon_points, const bool is_holonomic); - - // Creating routines - void createVelocityPolygon(const std::string & action_type, const bool is_holonomic); - - // Wait until polygon will be received - bool waitPolygon( - const std::chrono::nanoseconds & timeout, - std::vector & poly); - - std::shared_ptr test_node_; - - std::shared_ptr velocity_polygon_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; -}; // Tester - -Tester::Tester() -{ - test_node_ = std::make_shared(); - - tf_buffer_ = std::make_shared(test_node_->get_clock()); - tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model - tf_listener_ = std::make_shared(*tf_buffer_); -} - -Tester::~Tester() -{ - velocity_polygon_.reset(); - - test_node_.reset(); - - tf_listener_.reset(); - tf_buffer_.reset(); -} - -void Tester::setCommonParameters(const std::string & polygon_name, const std::string & action_type) -{ - test_node_->declare_parameter( - polygon_name + ".action_type", rclcpp::ParameterValue(action_type)); - test_node_->set_parameter( - rclcpp::Parameter(polygon_name + ".action_type", action_type)); - - test_node_->declare_parameter( - polygon_name + ".min_points", rclcpp::ParameterValue(MIN_POINTS)); - test_node_->set_parameter( - rclcpp::Parameter(polygon_name + ".min_points", MIN_POINTS)); - - test_node_->declare_parameter( - polygon_name + ".visualize", rclcpp::ParameterValue(true)); - test_node_->set_parameter( - rclcpp::Parameter(polygon_name + ".visualize", true)); - - test_node_->declare_parameter( - polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(POLYGON_PUB_TOPIC)); - test_node_->set_parameter( - rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC)); -} - -void Tester::setVelocityPolygonParameters(const bool is_holonomic) -{ - test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".holonomic", rclcpp::ParameterValue(is_holonomic)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".holonomic", is_holonomic)); - - std::vector velocity_polygons = - {SUB_POLYGON_FORWARD_NAME, SUB_POLYGON_BACKWARD_NAME}; - - if (is_holonomic) { - // Direction angle range for holonomic type - // - // ^OY - // | - // | - // 0.75pi (left) 0.25pi - // --------------- <- robot footprint - // | \ | / | - // (backward) | \ | / | (forward) - // --------pi--|------o------|---------->OX - // | / | \ | - // | / | \ | - // -------------- - // -0.75pi (right) -0.25pi - // | - addPolygonVelocitySubPolygon( - SUB_POLYGON_FORWARD_NAME, 0.0, 0.5, -1.0, 1.0, -M_PI_4, M_PI_4, FORWARD_POLYGON_STR, - is_holonomic); - addPolygonVelocitySubPolygon( - SUB_POLYGON_BACKWARD_NAME, -0.5, 0.0, -1.0, 1.0, 0.75 * M_PI, -0.75 * M_PI, - BACKWARD_POLYGON_STR, - is_holonomic); - addPolygonVelocitySubPolygon( - SUB_POLYGON_LEFT_NAME, -0.5, 0.5, -1.0, 1.0, M_PI_4, 0.75 * M_PI, LEFT_POLYGON_STR, - is_holonomic); - addPolygonVelocitySubPolygon( - SUB_POLYGON_RIGHT_NAME, -0.5, 0.5, -1.0, 1.0, -0.75 * M_PI, -M_PI_4, - RIGHT_POLYGON_STR, is_holonomic); - - velocity_polygons = {SUB_POLYGON_FORWARD_NAME, SUB_POLYGON_BACKWARD_NAME, SUB_POLYGON_LEFT_NAME, - SUB_POLYGON_RIGHT_NAME}; - } else { - // draw forward and backward polygon - addPolygonVelocitySubPolygon( - SUB_POLYGON_FORWARD_NAME, 0.0, 0.5, -1.0, 1.0, 0.0, 0.0, FORWARD_POLYGON_STR, - is_holonomic); - addPolygonVelocitySubPolygon( - SUB_POLYGON_BACKWARD_NAME, -0.5, 0.0, -1.0, 1.0, 0.0, 0.0, BACKWARD_POLYGON_STR, - is_holonomic); - velocity_polygons = {SUB_POLYGON_FORWARD_NAME, SUB_POLYGON_BACKWARD_NAME}; - } - - test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".velocity_polygons", rclcpp::ParameterValue(velocity_polygons)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".velocity_polygons", velocity_polygons)); -} - -void Tester::addPolygonVelocitySubPolygon( - const std::string & sub_polygon_name, - const double linear_min, const double linear_max, - const double theta_min, const double theta_max, - const double direction_start_angle, const double direction_end_angle, - const std::string & polygon_points, const bool is_holonomic) -{ - test_node_->declare_parameter( - std::string(POLYGON_NAME) + "." + sub_polygon_name + ".points", - rclcpp::ParameterValue(polygon_points)); - test_node_->set_parameter( - rclcpp::Parameter( - std::string( - POLYGON_NAME) + - "." + sub_polygon_name + ".points", - polygon_points)); - - test_node_->declare_parameter( - std::string(POLYGON_NAME) + "." + sub_polygon_name + ".linear_min", - rclcpp::ParameterValue(linear_min)); - test_node_->set_parameter( - rclcpp::Parameter( - std::string( - POLYGON_NAME) + - "." + sub_polygon_name + ".linear_min", - linear_min)); - - test_node_->declare_parameter( - std::string(POLYGON_NAME) + "." + sub_polygon_name + ".linear_max", - rclcpp::ParameterValue(linear_max)); - test_node_->set_parameter( - rclcpp::Parameter( - std::string( - POLYGON_NAME) + - "." + sub_polygon_name + ".linear_max", - linear_max)); - - test_node_->declare_parameter( - std::string(POLYGON_NAME) + "." + sub_polygon_name + ".theta_min", - rclcpp::ParameterValue(theta_min)); - test_node_->set_parameter( - rclcpp::Parameter( - std::string(POLYGON_NAME) + "." + sub_polygon_name + ".theta_min", - theta_min)); - - test_node_->declare_parameter( - std::string(POLYGON_NAME) + "." + sub_polygon_name + ".theta_max", - rclcpp::ParameterValue(theta_max)); - test_node_->set_parameter( - rclcpp::Parameter( - std::string(POLYGON_NAME) + "." + sub_polygon_name + ".theta_max", - theta_max)); - - if (is_holonomic) { - test_node_->declare_parameter( - std::string( - POLYGON_NAME) + - "." + sub_polygon_name + ".direction_end_angle", - rclcpp::ParameterValue(direction_end_angle)); - test_node_->set_parameter( - rclcpp::Parameter( - std::string(POLYGON_NAME) + "." + sub_polygon_name + ".direction_end_angle", - direction_end_angle)); - - test_node_->declare_parameter( - std::string( - POLYGON_NAME) + - "." + sub_polygon_name + ".direction_start_angle", - rclcpp::ParameterValue(direction_start_angle)); - test_node_->set_parameter( - rclcpp::Parameter( - std::string(POLYGON_NAME) + "." + sub_polygon_name + - ".direction_start_angle", - direction_start_angle)); - } -} - -void Tester::createVelocityPolygon(const std::string & action_type, const bool is_holonomic) -{ - setCommonParameters(POLYGON_NAME, action_type); - setVelocityPolygonParameters(is_holonomic); - - velocity_polygon_ = std::make_shared( - test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); - ASSERT_TRUE(velocity_polygon_->configure()); - velocity_polygon_->activate(); -} - -bool Tester::waitPolygon( - const std::chrono::nanoseconds & timeout, - std::vector & poly) -{ - rclcpp::Time start_time = test_node_->now(); - while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { - velocity_polygon_->getPolygon(poly); - if (poly.size() > 0) { - return true; - } - rclcpp::spin_some(test_node_->get_node_base_interface()); - std::this_thread::sleep_for(10ms); - } - return false; -} - -TEST_F(Tester, testVelocityPolygonGetStopParameters) -{ - createVelocityPolygon("stop", IS_NOT_HOLONOMIC); - - // Check that common parameters set correctly - EXPECT_EQ(velocity_polygon_->getName(), POLYGON_NAME); - EXPECT_EQ(velocity_polygon_->getActionType(), nav2_collision_monitor::STOP); - EXPECT_EQ(velocity_polygon_->getMinPoints(), MIN_POINTS); - EXPECT_EQ(velocity_polygon_->isVisualize(), true); -} - -TEST_F(Tester, testVelocityPolygonGetSlowdownParameters) -{ - createVelocityPolygon("slowdown", IS_NOT_HOLONOMIC); - - // Check that common parameters set correctly - EXPECT_EQ(velocity_polygon_->getName(), POLYGON_NAME); - EXPECT_EQ(velocity_polygon_->getActionType(), nav2_collision_monitor::SLOWDOWN); - EXPECT_EQ(velocity_polygon_->getMinPoints(), MIN_POINTS); - EXPECT_EQ(velocity_polygon_->isVisualize(), true); -} - -TEST_F(Tester, testVelocityPolygonParameters) -{ - createVelocityPolygon("stop", IS_NOT_HOLONOMIC); - - // Check velocity polygon parameters - EXPECT_EQ(velocity_polygon_->isHolonomic(), IS_NOT_HOLONOMIC); - ASSERT_EQ(velocity_polygon_->getSubPolygons().size(), 2u); -} - -TEST_F(Tester, testHolonomicVelocityPolygonParameters) -{ - createVelocityPolygon("stop", IS_HOLONOMIC); - - // Check velocity polygon parameters - EXPECT_EQ(velocity_polygon_->isHolonomic(), IS_HOLONOMIC); - ASSERT_EQ(velocity_polygon_->getSubPolygons().size(), 4u); -} - -TEST_F(Tester, testVelocityPolygonOutOfRangeVelocity) -{ - createVelocityPolygon("stop", IS_NOT_HOLONOMIC); - - // Check velocity polygon parameters - EXPECT_EQ(velocity_polygon_->isHolonomic(), IS_NOT_HOLONOMIC); - ASSERT_EQ(velocity_polygon_->getSubPolygons().size(), 2u); - - // Check that polygon is empty before the first cmd_vel received - std::vector poly; - velocity_polygon_->getPolygon(poly); - ASSERT_EQ(poly.size(), 0u); - - - // Publish out of range cmd_vel(linear) and check that polygon is still emtpy - nav2_collision_monitor::Velocity vel{0.6, 0.0, 0.0}; - velocity_polygon_->updatePolygon(vel); - ASSERT_FALSE(waitPolygon(500ms, poly)); - ASSERT_EQ(poly.size(), 0u); - - // Publish out of range cmd_vel(rotation) and check that polygon is still emtpy - vel = {0.3, 0.0, 1.5}; - velocity_polygon_->updatePolygon(vel); - ASSERT_FALSE(waitPolygon(500ms, poly)); - ASSERT_EQ(poly.size(), 0u); - - // Publish a valid cmd_vel and check that polygon is correct - vel = {0.3, 0.0, 0.0}; // 0.3 m/s forward movement - velocity_polygon_->updatePolygon(vel); - ASSERT_TRUE(waitPolygon(500ms, poly)); - ASSERT_EQ(poly.size(), 4u); -} - -TEST_F(Tester, testVelocityPolygonVelocitySwitching) -{ - createVelocityPolygon("stop", IS_NOT_HOLONOMIC); - - // Check velocity polygon parameters - EXPECT_EQ(velocity_polygon_->isHolonomic(), IS_NOT_HOLONOMIC); - ASSERT_EQ(velocity_polygon_->getSubPolygons().size(), 2u); - - // Check that polygon is empty before the first cmd_vel received - std::vector poly; - velocity_polygon_->getPolygon(poly); - ASSERT_EQ(poly.size(), 0u); - - // Publish cmd_vel (forward) and check that polygon is correct - nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; - velocity_polygon_->updatePolygon(vel); - ASSERT_TRUE(waitPolygon(500ms, poly)); - ASSERT_EQ(poly.size(), 4u); - EXPECT_NEAR(poly[0].x, FORWARD_POLYGON[0], EPSILON); - EXPECT_NEAR(poly[0].y, FORWARD_POLYGON[1], EPSILON); - EXPECT_NEAR(poly[1].x, FORWARD_POLYGON[2], EPSILON); - EXPECT_NEAR(poly[1].y, FORWARD_POLYGON[3], EPSILON); - EXPECT_NEAR(poly[2].x, FORWARD_POLYGON[4], EPSILON); - EXPECT_NEAR(poly[2].y, FORWARD_POLYGON[5], EPSILON); - EXPECT_NEAR(poly[3].x, FORWARD_POLYGON[6], EPSILON); - EXPECT_NEAR(poly[3].y, FORWARD_POLYGON[7], EPSILON); - - // Publish cmd_vel (backward) and check that polygon is correct - vel = {-0.3, 0.0, 0.0}; // 0.3 m/s backward movement - velocity_polygon_->updatePolygon(vel); - ASSERT_TRUE(waitPolygon(500ms, poly)); - ASSERT_EQ(poly.size(), 4u); - EXPECT_NEAR(poly[0].x, BACKWARD_POLYGON[0], EPSILON); - EXPECT_NEAR(poly[0].y, BACKWARD_POLYGON[1], EPSILON); - EXPECT_NEAR(poly[1].x, BACKWARD_POLYGON[2], EPSILON); - EXPECT_NEAR(poly[1].y, BACKWARD_POLYGON[3], EPSILON); - EXPECT_NEAR(poly[2].x, BACKWARD_POLYGON[4], EPSILON); - EXPECT_NEAR(poly[2].y, BACKWARD_POLYGON[5], EPSILON); - EXPECT_NEAR(poly[3].x, BACKWARD_POLYGON[6], EPSILON); - EXPECT_NEAR(poly[3].y, BACKWARD_POLYGON[7], EPSILON); -} - -TEST_F(Tester, testVelocityPolygonHolonomicVelocitySwitching) -{ - createVelocityPolygon("stop", IS_HOLONOMIC); - - // Check velocity polygon parameters - EXPECT_EQ(velocity_polygon_->isHolonomic(), IS_HOLONOMIC); - ASSERT_EQ(velocity_polygon_->getSubPolygons().size(), 4u); - - // Check that polygon is empty before the first cmd_vel received - std::vector poly; - velocity_polygon_->getPolygon(poly); - ASSERT_EQ(poly.size(), 0u); - - // Publish cmd_vel (forward) and check that polygon is correct - nav2_collision_monitor::Velocity vel{0.3, 0.0, 0.0}; - velocity_polygon_->updatePolygon(vel); - ASSERT_TRUE(waitPolygon(500ms, poly)); - ASSERT_EQ(poly.size(), 4u); - EXPECT_NEAR(poly[0].x, FORWARD_POLYGON[0], EPSILON); - EXPECT_NEAR(poly[0].y, FORWARD_POLYGON[1], EPSILON); - EXPECT_NEAR(poly[1].x, FORWARD_POLYGON[2], EPSILON); - EXPECT_NEAR(poly[1].y, FORWARD_POLYGON[3], EPSILON); - EXPECT_NEAR(poly[2].x, FORWARD_POLYGON[4], EPSILON); - EXPECT_NEAR(poly[2].y, FORWARD_POLYGON[5], EPSILON); - EXPECT_NEAR(poly[3].x, FORWARD_POLYGON[6], EPSILON); - EXPECT_NEAR(poly[3].y, FORWARD_POLYGON[7], EPSILON); - - // Publish cmd_vel (backward) and check that polygon is correct - vel = {-0.3, 0.0, 0.0}; // 0.3 m/s backward movement - velocity_polygon_->updatePolygon(vel); - ASSERT_TRUE(waitPolygon(500ms, poly)); - ASSERT_EQ(poly.size(), 4u); - EXPECT_NEAR(poly[0].x, BACKWARD_POLYGON[0], EPSILON); - EXPECT_NEAR(poly[0].y, BACKWARD_POLYGON[1], EPSILON); - EXPECT_NEAR(poly[1].x, BACKWARD_POLYGON[2], EPSILON); - EXPECT_NEAR(poly[1].y, BACKWARD_POLYGON[3], EPSILON); - EXPECT_NEAR(poly[2].x, BACKWARD_POLYGON[4], EPSILON); - EXPECT_NEAR(poly[2].y, BACKWARD_POLYGON[5], EPSILON); - EXPECT_NEAR(poly[3].x, BACKWARD_POLYGON[6], EPSILON); - EXPECT_NEAR(poly[3].y, BACKWARD_POLYGON[7], EPSILON); - - // Publish cmd_vel (left) and check that polygon is correct - vel = {0.0, 0.3, 0.0}; // 0.3 m/s left movement - velocity_polygon_->updatePolygon(vel); - ASSERT_TRUE(waitPolygon(500ms, poly)); - ASSERT_EQ(poly.size(), 4u); - EXPECT_NEAR(poly[0].x, LEFT_POLYGON[0], EPSILON); - EXPECT_NEAR(poly[0].y, LEFT_POLYGON[1], EPSILON); - EXPECT_NEAR(poly[1].x, LEFT_POLYGON[2], EPSILON); - EXPECT_NEAR(poly[1].y, LEFT_POLYGON[3], EPSILON); - EXPECT_NEAR(poly[2].x, LEFT_POLYGON[4], EPSILON); - EXPECT_NEAR(poly[2].y, LEFT_POLYGON[5], EPSILON); - EXPECT_NEAR(poly[3].x, LEFT_POLYGON[6], EPSILON); - EXPECT_NEAR(poly[3].y, LEFT_POLYGON[7], EPSILON); - - // Publish cmd_vel (right) and check that polygon is correct - vel = {0.0, -0.3, 0.0}; // 0.3 m/s right movement - velocity_polygon_->updatePolygon(vel); - ASSERT_TRUE(waitPolygon(500ms, poly)); - ASSERT_EQ(poly.size(), 4u); - EXPECT_NEAR(poly[0].x, RIGHT_POLYGON[0], EPSILON); - EXPECT_NEAR(poly[0].y, RIGHT_POLYGON[1], EPSILON); - EXPECT_NEAR(poly[1].x, RIGHT_POLYGON[2], EPSILON); - EXPECT_NEAR(poly[1].y, RIGHT_POLYGON[3], EPSILON); - EXPECT_NEAR(poly[2].x, RIGHT_POLYGON[4], EPSILON); - EXPECT_NEAR(poly[2].y, RIGHT_POLYGON[5], EPSILON); - EXPECT_NEAR(poly[3].x, RIGHT_POLYGON[6], EPSILON); - EXPECT_NEAR(poly[3].y, RIGHT_POLYGON[7], EPSILON); -} - - -int main(int argc, char ** argv) -{ - // Initialize the system - testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - - // Actual testing - bool test_result = RUN_ALL_TESTS(); - - // Shutdown - rclcpp::shutdown(); - - return test_result; -}