diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 8544429466a..2608df9b7f5 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -43,6 +43,18 @@ set(library_name ${executable_name}_core) add_library(${library_name} SHARED src/collision_monitor_node.cpp src/polygon.cpp + src/velocity_polygon.cpp + src/circle.cpp + src/source.cpp + src/scan.cpp + src/pointcloud.cpp + src/range.cpp + src/kinematics.cpp +) +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 @@ -51,8 +63,11 @@ add_library(${library_name} SHARED src/kinematics.cpp ) -add_executable(${executable_name} - src/main.cpp +add_executable(${monitor_executable_name} + src/collision_monitor_main.cpp +) +add_executable(${detector_executable_name} + src/collision_detector_main.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 54841675e90..e87b2e9bc96 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -27,9 +27,11 @@ The following models of safety behaviors are employed by Collision Monitor: The zones around the robot can take the following shapes: -* Arbitrary user-defined polygon relative to the robot base frame. -* Circle: is made for the best performance and could be used in the cases where the zone or robot could be approximated by round shape. -* Robot footprint polygon, which is used in the approach behavior model only. Will use the footprint topic to allow it to be dynamically adjusted over time. +* 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: @@ -42,10 +44,14 @@ 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. -![HDL.png](doc/HLD.png) +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. +![dexory_velocity_polygon.gif](doc/dexory_velocity_polygon.gif) + -## Configuration +### 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 new file mode 100644 index 00000000000..4fe2e1ad5ce Binary files /dev/null and b/nav2_collision_monitor/doc/dexory_velocity_polygon.gif 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 new file mode 100644 index 00000000000..fae479d6504 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -0,0 +1,164 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// Copyright (c) 2023 Pixel Robotics GmbH +// +// 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__COLLISION_DETECTOR_NODE_HPP_ +#define NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "tf2/time.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_msgs/msg/collision_detector_state.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#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" +#include "nav2_collision_monitor/range.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Collision Monitor ROS2 node + */ +class CollisionDetector : public nav2_util::LifecycleNode +{ +public: + /** + * @brief Constructor for the nav2_collision_monitor::CollisionDetector + * @param options Additional options to control creation of the node. + */ + explicit CollisionDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /** + * @brief Destructor for the nav2_collision_monitor::CollisionDetector + */ + ~CollisionDetector(); + +protected: + /** + * @brief: Initializes and obtains ROS-parameters, creates main subscribers and publishers, + * creates polygons and data sources objects + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Activates LifecyclePublishers, polygons and main processor, creates bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Resets all subscribers/publishers, polygons/data sources arrays + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called in shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + +protected: + /** + * @brief Supporting routine obtaining all ROS-parameters + * @return True if all parameters were obtained or false in failure case + */ + bool getParameters(); + /** + * @brief Supporting routine creating and configuring all polygons + * @param base_frame_id Robot base frame ID + * @param transform_tolerance Transform tolerance + * @return True if all polygons were configured successfully or false in failure case + */ + bool configurePolygons( + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance); + /** + * @brief Supporting routine creating and configuring all data sources + * @param base_frame_id Robot base frame ID + * @param odom_frame_id Odometry frame ID. Used as global frame to get + * source->base time interpolated transform. + * @param transform_tolerance Transform tolerance + * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time + * @return True if all sources were configured successfully or false in failure case + */ + bool configureSources( + const std::string & base_frame_id, + const std::string & odom_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); + + /** + * @brief Main processing routine + */ + void process(); + + /** + * @brief Polygons publishing routine. Made for visualization. + */ + void publishPolygons() const; + + // ----- Variables ----- + + /// @brief TF buffer + std::shared_ptr tf_buffer_; + /// @brief TF listener + std::shared_ptr tf_listener_; + + /// @brief Polygons array + std::vector> polygons_; + /// @brief Data sources array + std::vector> sources_; + + /// @brief collision monitor state publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + state_pub_; + /// @brief Collision points marker publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + collision_points_marker_pub_; + /// @brief timer that runs actions + rclcpp::TimerBase::SharedPtr timer_; + + /// @brief main loop frequency + double frequency_; +}; // class CollisionDetector + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_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 2aed9920d0a..b0f08fee16d 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 @@ -32,6 +32,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 97423dc411d..34e13497d34 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -113,7 +113,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 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 00000000000..e4c65bebad5 --- /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_ diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 1b0c36529e7..8ac2150ed15 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" and "slowdown" 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" @@ -38,6 +39,43 @@ collision_monitor: simulation_time_step: 0.1 max_points: 5 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 new file mode 100644 index 00000000000..4f485520c76 --- /dev/null +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -0,0 +1,392 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// Copyright (c) 2023 Pixel Robotics GmbH +// +// 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/collision_detector_node.hpp" + +#include +#include +#include + +#include "tf2_ros/create_timer_ros.h" + +#include "nav2_util/node_utils.hpp" + +using namespace std::chrono_literals; + +namespace nav2_collision_monitor +{ + +CollisionDetector::CollisionDetector(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("collision_detector", "", options) +{ +} + +CollisionDetector::~CollisionDetector() +{ + polygons_.clear(); + sources_.clear(); +} + +nav2_util::CallbackReturn +CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(this->get_clock()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), + this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + + state_pub_ = this->create_publisher( + "collision_detector_state", rclcpp::SystemDefaultsQoS()); + + collision_points_marker_pub_ = this->create_publisher( + "~/collision_points_marker", 1); + + // Obtaining ROS parameters + if (!getParameters()) { + return nav2_util::CallbackReturn::FAILURE; + } + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + // Activating lifecycle publisher + state_pub_->on_activate(); + collision_points_marker_pub_->on_activate(); + + // Activating polygons + for (std::shared_ptr polygon : polygons_) { + polygon->activate(); + } + + // Creating timer + timer_ = this->create_timer( + std::chrono::duration{1.0 / frequency_}, + std::bind(&CollisionDetector::process, this)); + + // Creating bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + // Resetting timer + timer_.reset(); + + // Deactivating lifecycle publishers + state_pub_->on_deactivate(); + collision_points_marker_pub_->on_deactivate(); + + // Deactivating polygons + for (std::shared_ptr polygon : polygons_) { + polygon->deactivate(); + } + + // Destroying bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionDetector::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + state_pub_.reset(); + collision_points_marker_pub_.reset(); + + polygons_.clear(); + sources_.clear(); + + tf_listener_.reset(); + tf_buffer_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionDetector::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} + +bool CollisionDetector::getParameters() +{ + std::string base_frame_id, odom_frame_id; + tf2::Duration transform_tolerance; + rclcpp::Duration source_timeout(2.0, 0.0); + + auto node = shared_from_this(); + + nav2_util::declare_parameter_if_not_declared( + node, "frequency", rclcpp::ParameterValue(10.0)); + frequency_ = get_parameter("frequency").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, "base_frame_id", rclcpp::ParameterValue("base_footprint")); + base_frame_id = get_parameter("base_frame_id").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, "odom_frame_id", rclcpp::ParameterValue("odom")); + odom_frame_id = get_parameter("odom_frame_id").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, "transform_tolerance", rclcpp::ParameterValue(0.1)); + transform_tolerance = + tf2::durationFromSec(get_parameter("transform_tolerance").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "source_timeout", rclcpp::ParameterValue(2.0)); + source_timeout = + rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "base_shift_correction", rclcpp::ParameterValue(true)); + const bool base_shift_correction = + get_parameter("base_shift_correction").as_bool(); + + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + + if (!configureSources( + base_frame_id, odom_frame_id, transform_tolerance, source_timeout, + base_shift_correction)) + { + return false; + } + + return true; +} + +bool CollisionDetector::configurePolygons( + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance) +{ + try { + auto node = shared_from_this(); + + // Leave it to be not initialized: to intentionally cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, "polygons", rclcpp::PARAMETER_STRING_ARRAY); + std::vector polygon_names = get_parameter("polygons").as_string_array(); + for (std::string polygon_name : polygon_names) { + // Leave it not initialized: the will cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, polygon_name + ".type", rclcpp::PARAMETER_STRING); + const std::string polygon_type = get_parameter(polygon_name + ".type").as_string(); + + if (polygon_type == "polygon") { + polygons_.push_back( + std::make_shared( + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + } else if (polygon_type == "circle") { + 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(), + "[%s]: Unknown polygon type: %s", + polygon_name.c_str(), polygon_type.c_str()); + return false; + } + + // Configure last added polygon + if (!polygons_.back()->configure()) { + return false; + } + + // warn if the added polygon's action_type_ is not different than "none" + auto action_type = polygons_.back()->getActionType(); + if (action_type != DO_NOTHING) { + RCLCPP_ERROR( + get_logger(), + "[%s]: The action_type of the polygon is different than \"none\" which is " + "not supported in the collision detector.", + polygon_name.c_str()); + return false; + } + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); + return false; + } + + return true; +} + +bool CollisionDetector::configureSources( + const std::string & base_frame_id, + const std::string & odom_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) +{ + try { + auto node = shared_from_this(); + + // Leave it to be not initialized to intentionally cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY); + std::vector source_names = get_parameter("observation_sources").as_string_array(); + for (std::string source_name : source_names) { + nav2_util::declare_parameter_if_not_declared( + node, source_name + ".type", + rclcpp::ParameterValue("scan")); // Laser scanner by default + const std::string source_type = get_parameter(source_name + ".type").as_string(); + + if (source_type == "scan") { + std::shared_ptr s = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + + s->configure(); + + sources_.push_back(s); + } else if (source_type == "pointcloud") { + std::shared_ptr p = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + + p->configure(); + + sources_.push_back(p); + } else if (source_type == "range") { + std::shared_ptr r = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + + r->configure(); + + sources_.push_back(r); + } else { // Error if something else + RCLCPP_ERROR( + get_logger(), + "[%s]: Unknown source type: %s", + source_name.c_str(), source_type.c_str()); + return false; + } + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); + return false; + } + + return true; +} + +void CollisionDetector::process() +{ + // Current timestamp for all inner routines prolongation + rclcpp::Time curr_time = this->now(); + + // Points array collected from different data sources in a robot base frame + std::vector collision_points; + + std::unique_ptr state_msg = + std::make_unique(); + + // Fill collision_points array from different data sources + for (std::shared_ptr source : sources_) { + if (source->getEnabled()) { + if (!source->getData(curr_time, collision_points) && + source->getSourceTimeout().seconds() != 0.0) + { + RCLCPP_WARN( + get_logger(), + "Invalid source %s detected." + " Either due to data not published yet, or to lack of new data received within the" + " sensor timeout, or if impossible to transform data to base frame", + source->getSourceName().c_str()); + } + } + } + + if (collision_points_marker_pub_->get_subscription_count() > 0) { + // visualize collision points with markers + auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker marker; + marker.header.frame_id = get_parameter("base_frame_id").as_string(); + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = "collision_points"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.color.r = 1.0; + marker.color.a = 1.0; + marker.lifetime = rclcpp::Duration(0, 0); + + for (const auto & point : collision_points) { + geometry_msgs::msg::Point p; + p.x = point.x; + p.y = point.y; + p.z = 0.0; + marker.points.push_back(p); + } + marker_array->markers.push_back(marker); + collision_points_marker_pub_->publish(std::move(marker_array)); + } + + for (std::shared_ptr polygon : polygons_) { + if (!polygon->getEnabled()) { + continue; + } + state_msg->polygons.push_back(polygon->getName()); + state_msg->detections.push_back( + polygon->getPointsInside( + collision_points) >= polygon->getMinPoints()); + } + + state_pub_->publish(std::move(state_msg)); + + // Publish polygons for better visualization + publishPolygons(); +} + +void CollisionDetector::publishPolygons() const +{ + for (std::shared_ptr polygon : polygons_) { + if (polygon->getEnabled()) { + polygon->publish(); + } + } +} + +} // namespace nav2_collision_monitor + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_collision_monitor::CollisionDetector) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 57125501cfa..b67ec32f258 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -259,6 +259,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(), @@ -369,6 +373,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) break; } + // Update polygon coordinates + polygon->updatePolygon(cmd_vel_in); + const ActionType at = polygon->getActionType(); if (at == STOP || at == SLOWDOWN) { // Process STOP/SLOWDOWN for the selected polygon diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index d17c34b46af..ea9de22c35b 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -129,7 +129,16 @@ void Polygon::getPolygon(std::vector & poly) const poly = poly_; } -void Polygon::updatePolygon() +bool Polygon::isShapeSet() +{ + if (poly_.empty()) { + RCLCPP_WARN(logger_, "[%s]: Polygon shape is not set yet", polygon_name_.c_str()); + return false; + } + return true; +} + +void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/) { 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 new file mode 100644 index 00000000000..84779a1a2d4 --- /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 diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt index 77870826b88..cbbfb438549 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -25,6 +25,15 @@ target_link_libraries(polygons_test ${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 c25b713f101..27de82cbc80 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -62,7 +62,8 @@ enum PolygonType { POLYGON_UNKNOWN = 0, POLYGON = 1, - CIRCLE = 2 + CIRCLE = 2, + VELOCITY_POLYGON = 3 }; enum SourceType @@ -126,10 +127,18 @@ 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); @@ -272,6 +281,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")); @@ -315,6 +333,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) { @@ -371,6 +426,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 = @@ -917,6 +980,103 @@ TEST_F(Tester, testSourcesNotSet) cm_->cant_configure(); } +TEST_F(Tester, testCollisionPointsMarkers) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner for robot stop. + setCommonParameters(); + addSource(SCAN_NAME, SCAN); + setVectors({}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); + + publishCmdVel(0.5, 0.2, 0.1); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_NE(collision_points_marker_msg_->markers[0].points.size(), 0u); + // Stop Collision Monitor node + 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 87576ddda93..9a7ceead2a0 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -325,8 +325,9 @@ 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; @@ -488,10 +489,94 @@ TEST_F(Tester, testCircleUndeclaredRadius) TEST_F(Tester, testPolygonUpdate) { - createPolygon("approach"); + createPolygon("stop", false); + + std::vector poly; + polygon_->getPolygon(poly); + ASSERT_FALSE(polygon_->isShapeSet()); + ASSERT_EQ(poly.size(), 0u); + + // Publish incorrect shape and check that polygon was not updated + test_node_->publishPolygon(BASE_FRAME_ID, false); + ASSERT_FALSE(waitPolygon(100ms, poly)); + ASSERT_FALSE(polygon_->isShapeSet()); + + // Publush correct polygon and make shure that it was set correctly + test_node_->publishPolygon(BASE_FRAME_ID, true); + ASSERT_TRUE(waitPolygon(500ms, poly)); + ASSERT_TRUE(polygon_->isShapeSet()); + ASSERT_EQ(poly.size(), 4u); + EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0], EPSILON); + EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1], EPSILON); + EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2], EPSILON); + EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3], EPSILON); + EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4], EPSILON); + EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5], EPSILON); + EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6], EPSILON); + EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7], EPSILON); +} + +TEST_F(Tester, testPolygonTopicUpdateDifferentFrame) +{ + createPolygon("stop", false); + sendTransforms(0.1); + + std::vector poly; + polygon_->getPolygon(poly); + ASSERT_EQ(poly.size(), 0u); + + // Publush polygon in different frame and make shure that it was set correctly + test_node_->publishPolygon(BASE2_FRAME_ID, true); + ASSERT_TRUE(waitPolygon(500ms, poly)); + ASSERT_EQ(poly.size(), 4u); + EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0] + 0.1, EPSILON); + EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1] + 0.1, EPSILON); + EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2] + 0.1, EPSILON); + EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3] + 0.1, EPSILON); + EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4] + 0.1, EPSILON); + EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5] + 0.1, EPSILON); + EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6] + 0.1, EPSILON); + EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7] + 0.1, EPSILON); + + // 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); + // Check that polygon coordinates were updated correctly + ASSERT_TRUE(waitPolygon(500ms, poly)); + ASSERT_EQ(poly.size(), 4u); + EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0] + 0.2, EPSILON); + EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1] + 0.2, EPSILON); + EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2] + 0.2, EPSILON); + EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3] + 0.2, EPSILON); + EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4] + 0.2, EPSILON); + EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5] + 0.2, EPSILON); + EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6] + 0.2, EPSILON); + EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7] + 0.2, EPSILON); +} + +TEST_F(Tester, testPolygonTopicUpdateIncorrectFrame) +{ + createPolygon("stop", false); + sendTransforms(0.1); + + std::vector poly; + polygon_->getPolygon(poly); + ASSERT_EQ(poly.size(), 0u); + + // Publush polygon in incorrect frame and check that polygon was not updated + test_node_->publishPolygon("incorrect_frame", true); + ASSERT_FALSE(waitPolygon(100ms, poly)); +} + +TEST_F(Tester, testPolygonFootprintUpdate) +{ + createPolygon("approach", false); std::vector poly; polygon_->getPolygon(poly); + ASSERT_FALSE(polygon_->isShapeSet()); ASSERT_EQ(poly.size(), 0u); test_node_->publishFootprint(); 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 00000000000..7030ddd2b20 --- /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; +}