diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index f4c732285a..f52bc2d4cc 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -49,6 +49,7 @@ 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,6 +61,7 @@ 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 1c5b32dc7d..776043a278 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -36,6 +36,8 @@ 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: @@ -48,8 +50,12 @@ 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 (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. +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. ![HLD.png](doc/HLD.png) + +`VelocityPolygon` can be configured with multiple sub polygons and can switch between them based on the velocity. + + ### 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/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 28e62f0ff2..7c9785588e 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,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/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 7572abf8cd..a696c681ca 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 f63e5942d4..05f92364fb 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -142,7 +142,7 @@ class Polygon /** * @brief Updates polygon from footprint subscriber (if any) */ - void updatePolygon(); + virtual void updatePolygon(const Velocity & /*cmd_vel_in*/); /** * @brief Gets number of points inside given polygon @@ -217,6 +217,15 @@ class Polygon */ bool isPointInside(const Point & point) const; + /** + * @brief Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...] + * @param poly_string Input String containing the verteceis of the polygon + * @param polygon Output Point vector with all the vertecies of the polygon + * @return True if all parameters were obtained or false in failure case + */ + bool getPolygonFromString(std::string & poly_string, std::vector & polygon); + + // ----- Variables ----- /// @brief Collision Monitor node diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp new file mode 100644 index 0000000000..3943e8f0bf --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -0,0 +1,115 @@ +// 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_ \ No newline at end of file diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 3e506ed1d7..74db6c3e8a 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -11,9 +11,10 @@ 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. - # Footprint could be "polygon" type with dynamically set footprint from footprint_topic - # or "circle" type with static footprint set by radius. "footprint_topic" parameter + # (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 # to be ignored in circular case. + # (3) "velocity_polygon" type with dynamically set polygon from velocity_polygons polygons: ["PolygonStop"] PolygonStop: type: "polygon" @@ -51,6 +52,42 @@ 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 b3c60cd7ef..8a2343d690 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -207,6 +207,10 @@ 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 2b2f58963e..e079492602 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(), @@ -458,7 +462,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) } // Update polygon coordinates - polygon->updatePolygon(); + polygon->updatePolygon(cmd_vel_in); 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 53fddccb59..8daddf1ddf 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -23,6 +23,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/array_parser.hpp" #include "nav2_collision_monitor/kinematics.hpp" using rcl_interfaces::msg::ParameterType; @@ -191,7 +192,7 @@ bool Polygon::isShapeSet() return true; } -void Polygon::updatePolygon() +void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/) { if (footprint_sub_ != nullptr) { // Get latest robot footprint from footprint subscriber @@ -580,4 +581,45 @@ inline bool Polygon::isPointInside(const Point & point) const return res; } +bool Polygon::getPolygonFromString( + std::string & poly_string, + std::vector & polygon) +{ + std::string error; + std::vector> vvf = nav2_util::parseVVF(poly_string, error); + + if (error != "") { + RCLCPP_ERROR( + logger_, "Error parsing polygon parameter %s: '%s'", + poly_string.c_str(), error.c_str()); + return false; + } + + // Check for minimum 4 points + if (vvf.size() <= 3) { + RCLCPP_ERROR( + logger_, + "Polygon must have at least three points."); + return false; + } + for (unsigned int i = 0; i < vvf.size(); i++) { + if (vvf[i].size() == 2) { + Point point; + point.x = vvf[i][0]; + point.y = vvf[i][1]; + polygon.push_back(point); + } else { + RCLCPP_ERROR( + logger_, + "Points in the polygon specification must be pairs of numbers" + "Found a point with %d numbers.", + static_cast(vvf[i].size())); + polygon.clear(); + return false; + } + } + + return true; +} + } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp new file mode 100644 index 0000000000..eeb5082aab --- /dev/null +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -0,0 +1,192 @@ +// 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 \ No newline at end of file diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt index f2b4619bbd..2f879e1d39 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -25,6 +25,15 @@ 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 8d518bc363..9b0e8689e3 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -68,7 +68,8 @@ enum PolygonType { POLYGON_UNKNOWN = 0, POLYGON = 1, - CIRCLE = 2 + CIRCLE = 2, + VELOCITY_POLYGON = 3 }; enum SourceType @@ -141,11 +142,19 @@ 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); @@ -323,6 +332,15 @@ 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")); @@ -381,6 +399,43 @@ 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) { @@ -437,6 +492,14 @@ 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 = @@ -1312,6 +1375,74 @@ 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 2ab46cdced..672563c38b 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -425,8 +425,10 @@ 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(); + polygon_->updatePolygon(vel); polygon_->getPolygon(footprint); if (footprint.size() > 0) { return true; @@ -673,8 +675,9 @@ TEST_F(Tester, testPolygonTopicUpdateDifferentFrame) // Move BASE2_FRAME_ID to 0.2 m away from BASE_FRAME_ID sendTransforms(0.2); - // updatePolygon() should update poly coordinates to correct ones in BASE_FRAME_ID - polygon_->updatePolygon(); + // 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); // 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 new file mode 100644 index 0000000000..e1efc8bd01 --- /dev/null +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -0,0 +1,577 @@ +// 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; +} \ No newline at end of file diff --git a/nav2_util/include/nav2_util/array_parser.hpp b/nav2_util/include/nav2_util/array_parser.hpp new file mode 100644 index 0000000000..6db612d7af --- /dev/null +++ b/nav2_util/include/nav2_util/array_parser.hpp @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * author: Dave Hershberger + */ +#ifndef NAV2_UTIL__ARRAY_PARSER_HPP_ +#define NAV2_UTIL__ARRAY_PARSER_HPP_ + +#include +#include + +namespace nav2_util +{ + +/** @brief Parse a vector of vectors of floats from a string. + * @param error_return If no error, error_return is set to "". If + * error, error_return will describe the error. + * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] + * + * On error, error_return is set and the return value could be + * anything, like part of a successful parse. */ +std::vector> parseVVF(const std::string & input, std::string & error_return); + +} // end namespace nav2_util + +#endif // NAV2_UTIL__ARRAY_PARSER_HPP_ \ No newline at end of file diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index a639a0e59e..77cd271918 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -1,4 +1,5 @@ add_library(${library_name} SHARED + array_parser.cpp costmap.cpp node_utils.cpp lifecycle_service_client.cpp diff --git a/nav2_util/src/array_parser.cpp b/nav2_util/src/array_parser.cpp new file mode 100644 index 0000000000..2c46d5ddd3 --- /dev/null +++ b/nav2_util/src/array_parser.cpp @@ -0,0 +1,105 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * author: Dave Hershberger + */ + +#include // for EOF +#include +#include +#include + +namespace nav2_util +{ + +/** @brief Parse a vector of vector of floats from a string. + * @param input + * @param error_return + * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */ +std::vector> parseVVF(const std::string & input, std::string & error_return) +{ + std::vector> result; + + std::stringstream input_ss(input); + int depth = 0; + std::vector current_vector; + while (!!input_ss && !input_ss.eof()) { + switch (input_ss.peek()) { + case EOF: + break; + case '[': + depth++; + if (depth > 2) { + error_return = "Array depth greater than 2"; + return result; + } + input_ss.get(); + current_vector.clear(); + break; + case ']': + depth--; + if (depth < 0) { + error_return = "More close ] than open ["; + return result; + } + input_ss.get(); + if (depth == 1) { + result.push_back(current_vector); + } + break; + case ',': + case ' ': + case '\t': + input_ss.get(); + break; + default: // All other characters should be part of the numbers. + if (depth != 2) { + std::stringstream err_ss; + err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'."; + error_return = err_ss.str(); + return result; + } + float value; + input_ss >> value; + if (!!input_ss) { + current_vector.push_back(value); + } + break; + } + } + + if (depth != 0) { + error_return = "Unterminated vector string."; + } else { + error_return = ""; + } + + return result; +} + +} // end namespace nav2_util \ No newline at end of file