diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 35cba4a2c94..5dfaf1576ac 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -15,6 +16,7 @@ find_package(nav2_common REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) ### Header ### @@ -31,12 +33,14 @@ set(dependencies rclcpp_components sensor_msgs geometry_msgs + std_msgs tf2 tf2_ros tf2_geometry_msgs nav2_util nav2_costmap_2d nav2_msgs + visualization_msgs ) set(monitor_executable_name collision_monitor) @@ -47,20 +51,24 @@ 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 src/pointcloud.cpp + src/polygon_source.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 src/pointcloud.cpp + src/polygon_source.cpp src/range.cpp src/kinematics.cpp ) @@ -127,6 +135,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) endif() ### Ament stuff ### diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 54841675e90..37046f884c3 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -1,5 +1,7 @@ # Nav2 Collision Monitor +## Collision Monitor + The Collision Monitor is a node providing an additional level of robot safety. It performs several collision avoidance related tasks using incoming data from the sensors, bypassing the costmap and trajectory planners, to monitor for and prevent potential collisions at the emergency-stop level. @@ -12,7 +14,11 @@ The costmaps / trajectory planners will handle most situations, but this is to h ![polygons.png](doc/polygons.png) -## Features +Demonstration of Collision Monitor abilities presented at 6th ROS Developers Day 2023, could be found below: + +[![cm-ros-devday.png](doc/cm_ros_devday.png)](https://www.youtube.com/watch?v=bWliK0PC5Ms) + +### Features The Collision Monitor uses polygons relative the robot's base frame origin to define "zones". Data that fall into these zones trigger an operation depending on the model being used. @@ -27,9 +33,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: @@ -37,26 +45,30 @@ The data may be obtained from different data sources: * PointClouds (`sensor_msgs::msg::PointCloud2` messages) * IR/Sonars (`sensor_msgs::msg::Range` messages) -## Design +### Design 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) -## Configuration +`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) -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. +### Configuration -## Metrics +Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://docs.nav2.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://docs.nav2.org/tutorials/docs/using_collision_monitor.html) pages. + + +### Metrics Designed to be used in wide variety of robots (incl. moving fast) and have a high level of reliability, Collision Monitor node should operate at fast rates. Typical one frame processing time is ~4-5ms for laser scanner (with 360 points) and ~4-20ms for PointClouds (having 24K points). The table below represents the details of operating times for different behavior models and shapes: -| | Stop/Slowdown model, Polygon area | Stop/Slowdown model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint | +| | Stop/Slowdown/Limit model, Polygon area | Stop/Slowdown/Limit model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint | |-|-----------------------------------|----------------------------------|-----------------------------------|----------------------------------| | LaserScan (360 points) processing time, ms | 4.45 | 4.45 | 4.93 | 4.86 | | PointCloud (24K points) processing time, ms | 4.94 | 4.06 | 20.67 | 10.87 | @@ -65,3 +77,21 @@ The following notes could be made: * Due to sheer speed, circle shapes are preferred for the approach behavior models if you can approximately model your robot as circular. * More points mean lower performance. Pointclouds could be culled or filtered before the Collision Monitor to improve performance. + + +## Collision Detector + +In some cases, the user may want to be informed about the detected obstacles without affecting the robot's velocity and instead take a different action within an external node. For example, the user may want to blink LEDs or sound an alarm when the robot is close to an obstacle. Another use case could be to detect data points in particular regions (e.g extremely close to the sensor) and warn of malfunctioning sensors. For this purpose, the Collision Detector node was introduced. + +It works similarly to the Collision Monitor, but does not affect the robot's velocity. It will only inform that data from the configured sources has been detected within the configured polygons via message to the `collision_detector_state` topic. + +### Features + +Similarly to the Collision Monitor, the Collision Detector uses polygons relative the robot's base frame origin to define "zones". +However, unlike the Collision Monitor that uses different behavior models, the Collision Detector does not use any of them and therefore the `action_type` should always be set to `none`. If set to anything else, it will implicitly be set to `none` and yield a warning. + +The zones around the robot and the data sources are the same as for the Collision Monitor, with the exception of the footprint polygon, which is not supported by the Collision Detector. + +### Configuration + +Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html). diff --git a/nav2_collision_monitor/doc/cm_ros_devday.png b/nav2_collision_monitor/doc/cm_ros_devday.png new file mode 100644 index 00000000000..63cc8fdea43 Binary files /dev/null and b/nav2_collision_monitor/doc/cm_ros_devday.png differ 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/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index 807d4b3dc7f..a468be7c880 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -19,6 +19,8 @@ #include #include +#include "std_msgs/msg/float32.hpp" + #include "nav2_collision_monitor/polygon.hpp" namespace nav2_collision_monitor @@ -26,7 +28,7 @@ namespace nav2_collision_monitor /** * @brief Circle shape implementaiton. - * For STOP/SLOWDOWN model it represents zone around the robot + * For STOP/SLOWDOWN/LIMIT model it represents zone around the robot * while for APPROACH model it represents robot footprint. */ class Circle : public Polygon @@ -66,22 +68,54 @@ class Circle : public Polygon */ int getPointsInside(const std::vector & points) const override; + /** + * @brief Returns true if circle radius is set. + * Otherwise, prints a warning and returns false. + */ + bool isShapeSet() override; + protected: /** * @brief Supporting routine obtaining polygon-specific ROS-parameters - * @param polygon_pub_topic Output name of polygon publishing topic + * @param polygon_sub_topic Input name of polygon subscription topic + * @param polygon_pub_topic Output name of polygon or radius publishing topic * @param footprint_topic Output name of footprint topic. For Circle returns empty string, * there is no footprint subscription in this class. * @return True if all parameters were obtained or false in failure case */ - bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) override; + bool getParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic) override; + + /** + * @brief Creates polygon or radius topic subscription + * @param polygon_sub_topic Output name of polygon or radius subscription topic. + * Empty, if no polygon subscription. + */ + void createSubscription(std::string & polygon_sub_topic) override; + + /** + * @brief Updates polygon from radius value + * @param radius New circle radius to update polygon + */ + void updatePolygon(double radius); + + /** + * @brief Dynamic circle radius callback + * @param msg Shared pointer to the radius value message + */ + void radiusCallback(std_msgs::msg::Float32::ConstSharedPtr msg); + // ----- Variables ----- /// @brief Radius of the circle double radius_; /// @brief (radius * radius) value. Stored for optimization. - double radius_squared_; + double radius_squared_ = -1.0; + /// @brief Radius subscription + rclcpp::Subscription::SharedPtr radius_sub_; }; // class Circle } // namespace nav2_collision_monitor 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 c4c5906b22a..7c9785588e2 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 @@ -28,14 +28,17 @@ #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" +#include "nav2_collision_monitor/polygon_source.hpp" namespace nav2_collision_monitor { @@ -147,6 +150,9 @@ class CollisionDetector : public nav2_util::LifecycleNode /// @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_; 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 da94591117e..d221014ed52 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 @@ -18,24 +18,28 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "nav2_util/lifecycle_node.hpp" -#include "nav2_util/robot_utils.hpp" +#include "nav2_msgs/msg/collision_monitor_state.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" +#include "nav2_collision_monitor/polygon_source.hpp" namespace nav2_collision_monitor { @@ -107,11 +111,13 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @param cmd_vel_in_topic Output name of cmd_vel_in topic * @param cmd_vel_out_topic Output name of cmd_vel_out topic * is required. + * @param state_topic topic name for publishing collision monitor state * @return True if all parameters were obtained or false in failure case */ bool getParameters( std::string & cmd_vel_in_topic, - std::string & cmd_vel_out_topic); + std::string & cmd_vel_out_topic, + std::string & state_topic); /** * @brief Supporting routine creating and configuring all polygons * @param base_frame_id Robot base frame ID @@ -146,39 +152,41 @@ class CollisionMonitor : public nav2_util::LifecycleNode void process(const Velocity & cmd_vel_in); /** - * @brief Processes the polygon of STOP and SLOWDOWN action type + * @brief Processes the polygon of STOP, SLOWDOWN and LIMIT action type * @param polygon Polygon to process - * @param collision_points Array of 2D obstacle points + * @param sources_collision_points_map Map containing source name as key and + * array of source's 2D obstacle points as value * @param velocity Desired robot velocity * @param robot_action Output processed robot action * @return True if returned action is caused by current polygon, otherwise false */ - bool processStopSlowdown( + bool processStopSlowdownLimit( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const; /** * @brief Processes APPROACH action type * @param polygon Polygon to process - * @param collision_points Array of 2D obstacle points + * @param sources_collision_points_map Map containing source name as key and + * array of source's 2D obstacle points as value * @param velocity Desired robot velocity * @param robot_action Output processed robot action * @return True if returned action is caused by current polygon, otherwise false */ bool processApproach( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const; /** - * @brief Prints robot action and polygon caused it (if it was) - * @param robot_action Robot action to print + * @brief Log and publish current robot action and polygon + * @param robot_action Robot action to notify * @param action_polygon Pointer to a polygon causing a selected action */ - void printAction( + void notifyActionState( const Action & robot_action, const std::shared_ptr action_polygon) const; /** @@ -205,6 +213,14 @@ class CollisionMonitor : public nav2_util::LifecycleNode /// @brief Output cmd_vel publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr cmd_vel_out_pub_; + /// @brief CollisionMonitor state publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + state_pub_; + + /// @brief Collision points marker publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + collision_points_marker_pub_; + /// @brief Whether main routine is active bool process_active_; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index 8e1d1caf43a..ea370f9acca 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -20,7 +20,6 @@ #include #include "sensor_msgs/msg/point_cloud2.hpp" -#include "nav2_util/robot_utils.hpp" #include "nav2_collision_monitor/source.hpp" @@ -70,10 +69,11 @@ class PointCloud : public Source * @param curr_time Current node time for data interpolation * @param data Array where the data from source to be added. * Added data is transformed to base_frame_id_ coordinate system at curr_time. + * @return false if an invalid source should block the robot */ - void getData( + bool getData( const rclcpp::Time & curr_time, - std::vector & data) const; + std::vector & data); protected: /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index d5843dd606c..6bb3b5dd011 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -18,10 +18,10 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" -#include "geometry_msgs/msg/polygon.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -36,7 +36,7 @@ namespace nav2_collision_monitor /** * @brief Basic polygon shape class. - * For STOP/SLOWDOWN model it represents zone around the robot + * For STOP/SLOWDOWN/LIMIT model it represents zone around the robot * while for APPROACH model it represents robot footprint. */ class Polygon @@ -86,22 +86,34 @@ class Polygon * @return Action type for current polygon */ ActionType getActionType() const; - /** + /** * @brief Obtains polygon enabled state * @return Whether polygon is enabled */ bool getEnabled() const; /** - * @brief Obtains polygon maximum points to enter inside polygon causing no action - * @return Maximum points to enter to current polygon and take no action + * @brief Obtains polygon minimum points to enter inside polygon causing the action + * @return Minimum number of data readings within a zone to trigger the action */ - int getMaxPoints() const; + int getMinPoints() const; /** * @brief Obtains speed slowdown ratio for current polygon. * Applicable for SLOWDOWN model. * @return Speed slowdown ratio */ double getSlowdownRatio() const; + /** + * @brief Obtains speed linear limit for current polygon. + * Applicable for LIMIT model. + * @return Speed linear limit + */ + double getLinearLimit() const; + /** + * @brief Obtains speed angular z limit for current polygon. + * Applicable for LIMIT model. + * @return Speed angular limit + */ + double getAngularLimit() const; /** * @brief Obtains required time before collision for current polygon. * Applicable for APPROACH model. @@ -115,10 +127,22 @@ class Polygon */ virtual void getPolygon(std::vector & poly) const; + /** + * @brief Obtains the name of the observation sources for current polygon. + * @return Names of the observation sources + */ + std::vector getSourcesNames() const; + + /** + * @brief Returns true if polygon points were set. + * Otherwise, prints a warning and returns false. + */ + virtual bool isShapeSet(); + /** * @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 @@ -128,44 +152,85 @@ class Polygon */ virtual int getPointsInside(const std::vector & points) const; + /** + * @brief Gets number of points inside given polygon + * @param sources_collision_points_map Map containing source name as key, + * and input array of source's points to be checked as value + * @return Number of points inside polygon, + * for sources in map that are associated with current polygon. + * If there are no points, returns zero value. + */ + virtual int getPointsInside( + const std::unordered_map> & sources_collision_points_map) const; + /** * @brief Obtains estimated (simulated) time before a collision. * Applicable for APPROACH model. - * @param collision_points Array of 2D obstacle points + * @param sources_collision_points_map Map containing source name as key, + * and input array of source's 2D obstacle points as value * @param velocity Simulated robot velocity * @return Estimated time before a collision. If there is no collision, * return value will be negative. */ double getCollisionTime( - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity) const; /** * @brief Publishes polygon message into a its own topic */ - void publish() const; + void publish(); protected: /** * @brief Supporting routine obtaining ROS-parameters common for all shapes - * @param polygon_pub_topic Output name of polygon publishing topic + * @param polygon_pub_topic Output name of polygon or radius subscription topic. + * Empty, if no polygon subscription. + * @param polygon_sub_topic Output name of polygon publishing topic + * @param footprint_topic Output name of footprint topic. + * Empty, if no footprint subscription. + * @param use_dynamic_sub If false, the parameter polygon_sub_topic or footprint_topic + * will not be declared * @return True if all parameters were obtained or false in failure case */ - bool getCommonParameters(std::string & polygon_pub_topic); + bool getCommonParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic, + bool use_dynamic_sub = false); /** * @brief Supporting routine obtaining polygon-specific ROS-parameters + * @param polygon_sub_topic Output name of polygon or radius subscription topic. + * Empty, if no polygon subscription. * @param polygon_pub_topic Output name of polygon publishing topic - * @param footprint_topic Output name of footprint topic. Empty, if no footprint subscription + * @param footprint_topic Output name of footprint topic. + * Empty, if no footprint subscription. * @return True if all parameters were obtained or false in failure case */ - virtual bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic); + virtual bool getParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic); /** - * @brief Checks if point is inside polygon - * @param point Given point to check - * @return True if given point is inside polygon, otherwise false + * @brief Creates polygon or radius topic subscription + * @param polygon_sub_topic Output name of polygon or radius subscription topic. + * Empty, if no polygon subscription. */ + virtual void createSubscription(std::string & polygon_sub_topic); + + /** + * @brief Updates polygon from geometry_msgs::msg::PolygonStamped message + * @param msg Message to update polygon from + */ + void updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg); + + /** + * @brief Dynamic polygon callback + * @param msg Shared pointer to the polygon message + */ + void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg); /** * @brief Callback executed when a parameter change is detected @@ -174,8 +239,21 @@ class Polygon rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( std::vector parameters); + /** + * @brief Checks if point is inside polygon + * @param point Given point to check + * @return True if given point is inside polygon, otherwise false + */ 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 @@ -184,24 +262,34 @@ class Polygon rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; /// @brief Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; - + // Basic parameters /// @brief Name of polygon std::string polygon_name_; /// @brief Action type for the polygon ActionType action_type_; - /// @brief Maximum number of data readings within a zone to not trigger the action - int max_points_; + /// @brief Minimum number of data readings within a zone to trigger the action + int min_points_; /// @brief Robot slowdown (share of its actual speed) double slowdown_ratio_; + /// @brief Robot linear limit + double linear_limit_; + /// @brief Robot angular limit + double angular_limit_; /// @brief Time before collision in seconds double time_before_collision_; /// @brief Time step for robot movement simulation double simulation_time_step_; - /// @brief Footprint subscriber - std::unique_ptr footprint_sub_; /// @brief Whether polygon is enabled bool enabled_; + /// @brief Wether the subscription to polygon topic has transient local QoS durability + bool polygon_subscribe_transient_local_; + /// @brief Polygon subscription + rclcpp::Subscription::SharedPtr polygon_sub_; + /// @brief Footprint subscriber + std::unique_ptr footprint_sub_; + /// @brief Name of the observation sources to check for polygon + std::vector sources_names_; // Global variables /// @brief TF buffer @@ -214,12 +302,12 @@ class Polygon // Visualization /// @brief Whether to publish the polygon bool visualize_; - /// @brief Polygon points stored for later publishing - geometry_msgs::msg::Polygon polygon_; + /// @brief Polygon, used for: 1. visualization; 2. storing latest dynamic polygon message + geometry_msgs::msg::PolygonStamped polygon_; /// @brief Polygon publisher for visualization purposes rclcpp_lifecycle::LifecyclePublisher::SharedPtr polygon_pub_; - /// @brief Polygon points (vertices) + /// @brief Polygon points (vertices) in a base_frame_id_ std::vector poly_; }; // class Polygon diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp new file mode 100644 index 00000000000..a9e492faf63 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp @@ -0,0 +1,115 @@ +// 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__POLYGON_SOURCE_HPP_ +#define NAV2_COLLISION_MONITOR__POLYGON_SOURCE_HPP_ + +#include +#include +#include + +#include "nav2_msgs/msg/polygon_instance_stamped.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" + +#include "nav2_collision_monitor/source.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Implementation for polygon source + */ +class PolygonSource : public Source +{ +public: + /** + * @brief PolygonSource constructor + * @param node Collision Monitor node pointer + * @param source_name Name of data source + * @param tf_buffer Shared pointer to a TF buffer + * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. + * @param global_frame_id Global frame ID for correct transform calculation + * @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 + */ + PolygonSource( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); + /** + * @brief PolygonSource destructor + */ + ~PolygonSource(); + + /** + * @brief Data source configuration routine. Obtains ROS-parameters + * and creates subscriber. + */ + void configure(); + + /** + * @brief Adds latest data from polygon source to the data array. + * @param curr_time Current node time for data interpolation + * @param data Array where the data from source to be added. + * Added data is transformed to base_frame_id_ coordinate system at curr_time. + * @return false if an invalid source should block the robot + */ + bool getData( + const rclcpp::Time & curr_time, + std::vector & data); + + /** + * @brief Converts a PolygonInstanceStamped to a std::vector + * @param polygon Input Polygon to be converted + * @param data Output vector of Point + */ + void convertPolygonStampedToPoints( + const geometry_msgs::msg::PolygonStamped & polygon, + std::vector & data) const; + +protected: + /** + * @brief Getting sensor-specific ROS-parameters + * @param source_topic Output name of source subscription topic + */ + void getParameters(std::string & source_topic); + + /** + * @brief PolygonSource data callback + * @param msg Shared pointer to PolygonSource message + */ + void dataCallback(nav2_msgs::msg::PolygonInstanceStamped::ConstSharedPtr msg); + + // ----- Variables ----- + + /// @brief PolygonSource data subscriber + rclcpp::Subscription::SharedPtr data_sub_; + + /// @brief Latest data obtained + std::vector data_; + + /// @brief distance between sampled points on polygon edges + double sampling_distance_; +}; // class PolygonSource + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__POLYGON_SOURCE_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp index 1deb80a448b..2bccaac108c 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -20,7 +20,6 @@ #include #include "sensor_msgs/msg/range.hpp" -#include "nav2_util/robot_utils.hpp" #include "nav2_collision_monitor/source.hpp" @@ -70,10 +69,11 @@ class Range : public Source * @param curr_time Current node time for data interpolation * @param data Array where the data from source to be added. * Added data is transformed to base_frame_id_ coordinate system at curr_time. + * @return false if an invalid source should block the robot */ - void getData( + bool getData( const rclcpp::Time & curr_time, - std::vector & data) const; + std::vector & data); protected: /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index 690a22564e5..5c57cdb0148 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -20,7 +20,6 @@ #include #include "sensor_msgs/msg/laser_scan.hpp" -#include "nav2_util/robot_utils.hpp" #include "nav2_collision_monitor/source.hpp" @@ -70,10 +69,11 @@ class Scan : public Source * @param curr_time Current node time for data interpolation * @param data Array where the data from source to be added. * Added data is transformed to base_frame_id_ coordinate system at curr_time. + * @return false if an invalid source should block the robot */ - void getData( + bool getData( const rclcpp::Time & curr_time, - std::vector & data) const; + std::vector & data); protected: /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 30d58d53bc1..4432ec243b1 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -24,9 +24,9 @@ #include "tf2/time.h" #include "tf2_ros/buffer.h" -#include "nav2_util/lifecycle_node.hpp" - #include "nav2_collision_monitor/types.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "std_msgs/msg/header.hpp" namespace nav2_collision_monitor { @@ -69,10 +69,11 @@ class Source * @param curr_time Current node time for data interpolation * @param data Array where the data from source to be added. * Added data is transformed to base_frame_id_ coordinate system at curr_time. + * @return false if an invalid source should block the robot */ - virtual void getData( + virtual bool getData( const rclcpp::Time & curr_time, - std::vector & data) const = 0; + std::vector & data) = 0; /** * @brief Obtains source enabled state @@ -80,6 +81,18 @@ class Source */ bool getEnabled() const; + /** + * @brief Obtains the name of the data source + * @return Name of the data source + */ + std::string getSourceName() const; + + /** + * @brief Obtains the source_timeout parameter of the data source + * @return source_timeout parameter value of the data source + */ + rclcpp::Duration getSourceTimeout() const; + protected: /** * @brief Source configuration routine. @@ -110,6 +123,21 @@ class Source rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( std::vector parameters); + /** + * @brief Obtain the transform to get data from source frame and time where it was received to the + * base frame and current time (if base_shift_correction_ is true) or the transform without time + * shift considered which is less accurate but much more faster option not dependent on state + * estimation frames. + * @param curr_time Current node time + * @param data_header Current header which contains the frame_id and the stamp + * @param tf_transform Output source->base_frame_id_ transform + * @return True if got correct transform, otherwise false + */ + bool getTransform( + const rclcpp::Time & curr_time, + const std_msgs::msg::Header & data_header, + tf2::Transform & tf_transform) const; + // ----- Variables ----- /// @brief Collision Monitor node diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp index aa0b9d729ba..ac93b916be7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp @@ -15,6 +15,8 @@ #ifndef NAV2_COLLISION_MONITOR__TYPES_HPP_ #define NAV2_COLLISION_MONITOR__TYPES_HPP_ +#include + namespace nav2_collision_monitor { @@ -65,7 +67,8 @@ enum ActionType DO_NOTHING = 0, // No action STOP = 1, // Stop the robot SLOWDOWN = 2, // Slowdown in percentage from current operating speed - APPROACH = 3 // Keep constant time interval before collision + APPROACH = 3, // Keep constant time interval before collision + LIMIT = 4 // Limit absolute velocity from current operating speed }; /// @brief Action for robot @@ -73,6 +76,7 @@ struct Action { ActionType action_type; Velocity req_vel; + std::string polygon_name; }; } // namespace nav2_collision_monitor 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/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py index ccd58c2e158..c16bec55a4d 100644 --- a/nav2_collision_monitor/launch/collision_detector_node.launch.py +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -25,7 +25,7 @@ from launch.substitutions import NotEqualsSubstitution from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace +from launch_ros.actions import PushROSNamespace from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml @@ -37,8 +37,7 @@ def generate_launch_description(): # Constant parameters lifecycle_nodes = ['collision_detector'] autostart = True - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Launch arguments # 1. Create the launch configuration variables @@ -51,68 +50,83 @@ def generate_launch_description(): # 2. Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='True', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', - default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + default_value=os.path.join( + package_dir, 'params', 'collision_monitor_params.yaml' + ), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', - description='Use composed bringup if True') + 'use_composition', + default_value='True', + description='Use composed bringup if True', + ) declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, - convert_types=True) + convert_types=True, + ) # Declare node launching commands load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ SetParameter('use_sim_time', use_sim_time), - PushRosNamespace( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), - namespace=namespace), + PushROSNamespace( + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_collision_detector', output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}], - remappings=remappings), + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + remappings=remappings, + ), Node( package='nav2_collision_monitor', executable='collision_detector', output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 parameters=[configured_params], - remappings=remappings) - ] + remappings=remappings, + ), + ], ) load_composable_nodes = GroupAction( condition=IfCondition(use_composition), actions=[ SetParameter('use_sim_time', use_sim_time), - PushRosNamespace( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), - namespace=namespace), + PushROSNamespace( + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), LoadComposableNodes( target_container=container_name_full, composable_node_descriptions=[ @@ -120,18 +134,22 @@ def generate_launch_description(): package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', name='lifecycle_manager_collision_detector', - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}], - remappings=remappings), + parameters=[ + {'autostart': autostart}, + {'node_names': lifecycle_nodes}, + ], + remappings=remappings, + ), ComposableNode( package='nav2_collision_monitor', plugin='nav2_collision_monitor::CollisionDetector', name='collision_detector', parameters=[configured_params], - remappings=remappings) + remappings=remappings, + ), ], - ) - ] + ), + ], ) ld = LaunchDescription() diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index e1a570b23b4..b00b43b2aa1 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -19,10 +19,14 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import NotEqualsSubstitution +from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node -from launch_ros.descriptions import ParameterFile +from launch_ros.actions import PushROSNamespace +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -33,58 +37,123 @@ def generate_launch_description(): # Constant parameters lifecycle_nodes = ['collision_monitor'] autostart = True + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Launch arguments # 1. Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) # 2. Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='True', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', - default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + default_value=os.path.join( + package_dir, 'params', 'collision_monitor_params.yaml' + ), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='True', + description='Use composed bringup if True', + ) - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'use_sim_time': use_sim_time} + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True), - allow_substs=True) - - # Nodes launching commands - start_lifecycle_manager_cmd = Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager', - output='screen', - emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]) - - start_collision_monitor_cmd = Node( - package='nav2_collision_monitor', - executable='collision_monitor', - output='screen', - emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[configured_params]) + param_rewrites={}, + convert_types=True, + ), + allow_substs=True, + ) + + # Declare node launching commands + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushROSNamespace( + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_collision_monitor', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + remappings=remappings, + ), + Node( + package='nav2_collision_monitor', + executable='collision_monitor', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params], + remappings=remappings, + ), + ], + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushROSNamespace( + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), + LoadComposableNodes( + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_collision_monitor', + parameters=[ + {'autostart': autostart}, + {'node_names': lifecycle_nodes}, + ], + remappings=remappings, + ), + ComposableNode( + package='nav2_collision_monitor', + plugin='nav2_collision_monitor::CollisionMonitor', + name='collision_monitor', + parameters=[configured_params], + remappings=remappings, + ), + ], + ), + ], + ) ld = LaunchDescription() @@ -92,9 +161,11 @@ def generate_launch_description(): ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) # Node launching commands - ld.add_action(start_lifecycle_manager_cmd) - ld.add_action(start_collision_monitor_cmd) + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) return ld diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 27b6171c087..1b5e8a34501 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.17 + 1.3.3 Collision Monitor Alexey Merzlyakov Steve Macenski @@ -17,9 +17,12 @@ tf2_geometry_msgs sensor_msgs geometry_msgs + std_msgs nav2_common nav2_util nav2_costmap_2d + nav2_msgs + visualization_msgs ament_cmake_gtest ament_lint_common diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index 218ce452397..eb3ee0a8739 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -9,17 +9,20 @@ collision_detector: polygons: ["PolygonFront"] PolygonFront: type: "polygon" - points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" action_type: "none" min_points: 4 visualize: True polygon_pub_topic: "polygon_front" + enabled: True observation_sources: ["scan"] scan: type: "scan" topic: "scan" + enabled: True pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" min_height: 0.1 max_height: 0.5 + enabled: True diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index a84d74c6590..d6b3cd25fda 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -1,50 +1,97 @@ collision_monitor: ros__parameters: - use_sim_time: True base_frame_id: "base_footprint" odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_raw" + cmd_vel_in_topic: "cmd_vel_smoothed" cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" transform_tolerance: 0.5 source_timeout: 5.0 base_shift_correction: True stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop" and "slowdown" action types, + # 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" - points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" action_type: "stop" - max_points: 3 + min_points: 4 visualize: True polygon_pub_topic: "polygon_stop" enabled: True PolygonSlow: type: "polygon" - points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4] + points: "[[0.4, 0.4], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.4]]" action_type: "slowdown" - max_points: 3 + min_points: 4 slowdown_ratio: 0.3 visualize: True polygon_pub_topic: "polygon_slowdown" enabled: True + PolygonLimit: + type: "polygon" + points: "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]" + action_type: "limit" + min_points: 4 + linear_limit: 0.4 + angular_limit: 0.5 + visualize: True + polygon_pub_topic: "polygon_limit" + enabled: True FootprintApproach: type: "polygon" action_type: "approach" footprint_topic: "/local_costmap/published_footprint" time_before_collision: 2.0 simulation_time_step: 0.1 - max_points: 5 + 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" - topic: "/scan" + topic: "scan" enabled: True pointcloud: type: "pointcloud" diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index 7dc97089677..353db72dc07 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -70,35 +70,110 @@ int Circle::getPointsInside(const std::vector & points) const return num; } -bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) +bool Circle::isShapeSet() +{ + if (radius_squared_ == -1.0) { + RCLCPP_WARN(logger_, "[%s]: Circle radius is not set yet", polygon_name_.c_str()); + return false; + } + return true; +} + +bool Circle::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"}; } - if (!getCommonParameters(polygon_pub_topic)) { - return false; - } - - // There is no footprint subscription for the Circle. Thus, set string as empty. - footprint_topic.clear(); + // Clear the polygon subscription topic. It will be set later, if necessary. + polygon_sub_topic.clear(); + bool use_dynamic_sub = true; // if getting parameter radius fails, use dynamic subscription try { // Leave it not initialized: the will cause an error if it will not set nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".radius", rclcpp::PARAMETER_DOUBLE); radius_ = node->get_parameter(polygon_name_ + ".radius").as_double(); radius_squared_ = radius_ * radius_; - } catch (const std::exception & ex) { - RCLCPP_ERROR( + use_dynamic_sub = false; + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { + RCLCPP_INFO( logger_, - "[%s]: Error while getting circle parameters: %s", - polygon_name_.c_str(), ex.what()); - return false; + "[%s]: Polygon circle radius is not defined. Using dynamic subscription instead.", + polygon_name_.c_str()); } - return true; + bool ret = true; + if (!getCommonParameters( + polygon_sub_topic, polygon_pub_topic, footprint_topic, use_dynamic_sub)) + { + if (use_dynamic_sub && polygon_sub_topic.empty()) { + RCLCPP_ERROR( + logger_, + "[%s]: Error while getting circle parameters: static radius and sub topic both not defined", + polygon_name_.c_str()); + } + ret = false; + } + + // There is no footprint subscription for the Circle. Thus, set string as empty. + footprint_topic.clear(); + + return ret; +} + +void Circle::createSubscription(std::string & polygon_sub_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!polygon_sub_topic.empty()) { + RCLCPP_INFO( + logger_, + "[%s]: Subscribing on %s topic for polygon", + polygon_name_.c_str(), polygon_sub_topic.c_str()); + rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + if (polygon_subscribe_transient_local_) { + polygon_qos.transient_local(); + } + radius_sub_ = node->create_subscription( + polygon_sub_topic, polygon_qos, + std::bind(&Circle::radiusCallback, this, std::placeholders::_1)); + } +} + +void Circle::updatePolygon(double radius) +{ + // Update circle radius + radius_ = radius; + radius_squared_ = radius_ * radius_; + + // Create a polygon from radius and store it + std::vector poly; + getPolygon(poly); + polygon_.polygon.points.clear(); // clear polygon points + for (const Point & p : poly) { + geometry_msgs::msg::Point32 p_s; + p_s.x = p.x; + p_s.y = p.y; + // p_s.z will remain 0.0 + polygon_.polygon.points.push_back(p_s); // add new points + } +} + +void Circle::radiusCallback(std_msgs::msg::Float32::ConstSharedPtr msg) +{ + RCLCPP_INFO( + logger_, + "[%s]: Polygon circle radius update has been arrived", + polygon_name_.c_str()); + updatePolygon(msg->data); } } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index e4ecf653f01..072795c7e6f 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -55,6 +55,9 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & state) 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()) { on_cleanup(state); @@ -71,6 +74,7 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) // Activating lifecycle publisher state_pub_->on_activate(); + collision_points_marker_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -98,6 +102,7 @@ CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // Deactivating lifecycle publishers state_pub_->on_deactivate(); + collision_points_marker_pub_->on_deactivate(); // Deactivating polygons for (std::shared_ptr polygon : polygons_) { @@ -116,6 +121,7 @@ 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(); @@ -163,10 +169,6 @@ bool CollisionDetector::getParameters() 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)) @@ -174,6 +176,10 @@ bool CollisionDetector::getParameters() return false; } + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + return true; } @@ -202,6 +208,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(), @@ -225,7 +235,6 @@ bool CollisionDetector::configurePolygons( polygon_name.c_str()); return false; } - } } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); @@ -279,6 +288,13 @@ bool CollisionDetector::configureSources( r->configure(); sources_.push_back(r); + } else if (source_type == "polygon") { + std::shared_ptr ps = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + ps->configure(); + + sources_.push_back(ps); } else { // Error if something else RCLCPP_ERROR( get_logger(), @@ -303,18 +319,61 @@ void CollisionDetector::process() // 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_) { - source->getData(curr_time, collision_points); + 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()); + } + } } - std::unique_ptr state_msg = - std::make_unique(); + 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); + marker.frame_locked = true; + + 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->getMaxPoints()); + polygon->getPointsInside( + collision_points) >= polygon->getMinPoints()); } state_pub_->publish(std::move(state_msg)); @@ -326,7 +385,9 @@ void CollisionDetector::process() void CollisionDetector::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - polygon->publish(); + if (polygon->getEnabled()) { + polygon->publish(); + } } } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 21569b96673..2341e0188c6 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -21,6 +21,7 @@ #include "tf2_ros/create_timer_ros.h" #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" #include "nav2_collision_monitor/kinematics.hpp" @@ -29,7 +30,7 @@ namespace nav2_collision_monitor CollisionMonitor::CollisionMonitor(const rclcpp::NodeOptions & options) : nav2_util::LifecycleNode("collision_monitor", "", options), - process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}}, + process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}, ""}, stop_stamp_{0, 0, get_clock()->get_clock_type()}, stop_pub_timeout_(1.0, 0.0) { } @@ -55,9 +56,10 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) std::string cmd_vel_in_topic; std::string cmd_vel_out_topic; + std::string state_topic; // Obtaining ROS parameters - if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic)) { + if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) { on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -68,6 +70,14 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) cmd_vel_out_pub_ = this->create_publisher( cmd_vel_out_topic, 1); + if (!state_topic.empty()) { + state_pub_ = this->create_publisher( + state_topic, 1); + } + + collision_points_marker_pub_ = this->create_publisher( + "~/collision_points_marker", 1); + return nav2_util::CallbackReturn::SUCCESS; } @@ -78,6 +88,10 @@ CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/) // Activating lifecycle publisher cmd_vel_out_pub_->on_activate(); + if (state_pub_) { + state_pub_->on_activate(); + } + collision_points_marker_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -106,7 +120,7 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/) process_active_ = false; // Reset action type to default after worker deactivating - robot_action_prev_ = {DO_NOTHING, {-1.0, -1.0, -1.0}}; + robot_action_prev_ = {DO_NOTHING, {-1.0, -1.0, -1.0}, ""}; // Deactivating polygons for (std::shared_ptr polygon : polygons_) { @@ -115,6 +129,10 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // Deactivating lifecycle publishers cmd_vel_out_pub_->on_deactivate(); + if (state_pub_) { + state_pub_->on_deactivate(); + } + collision_points_marker_pub_->on_deactivate(); // Destroying bond connection destroyBond(); @@ -129,6 +147,8 @@ CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/) cmd_vel_in_sub_.reset(); cmd_vel_out_pub_.reset(); + state_pub_.reset(); + collision_points_marker_pub_.reset(); polygons_.clear(); sources_.clear(); @@ -183,7 +203,8 @@ void CollisionMonitor::publishVelocity(const Action & robot_action) bool CollisionMonitor::getParameters( std::string & cmd_vel_in_topic, - std::string & cmd_vel_out_topic) + std::string & cmd_vel_out_topic, + std::string & state_topic) { std::string base_frame_id, odom_frame_id; tf2::Duration transform_tolerance; @@ -192,11 +213,14 @@ bool CollisionMonitor::getParameters( auto node = shared_from_this(); nav2_util::declare_parameter_if_not_declared( - node, "cmd_vel_in_topic", rclcpp::ParameterValue("cmd_vel_raw")); + node, "cmd_vel_in_topic", rclcpp::ParameterValue("cmd_vel_smoothed")); cmd_vel_in_topic = get_parameter("cmd_vel_in_topic").as_string(); nav2_util::declare_parameter_if_not_declared( node, "cmd_vel_out_topic", rclcpp::ParameterValue("cmd_vel")); cmd_vel_out_topic = get_parameter("cmd_vel_out_topic").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, "state_topic", rclcpp::ParameterValue("")); + state_topic = get_parameter("state_topic").as_string(); nav2_util::declare_parameter_if_not_declared( node, "base_frame_id", rclcpp::ParameterValue("base_footprint")); @@ -222,10 +246,6 @@ bool CollisionMonitor::getParameters( stop_pub_timeout_ = rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double()); - if (!configurePolygons(base_frame_id, transform_tolerance)) { - return false; - } - if ( !configureSources( base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) @@ -233,6 +253,10 @@ bool CollisionMonitor::getParameters( return false; } + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + return true; } @@ -261,6 +285,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(), @@ -326,6 +354,13 @@ bool CollisionMonitor::configureSources( r->configure(); sources_.push_back(r); + } else if (source_type == "polygon") { + std::shared_ptr ps = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + ps->configure(); + + sources_.push_back(ps); } else { // Error if something else RCLCPP_ERROR( get_logger(), @@ -353,19 +388,63 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) } // Points array collected from different data sources in a robot base frame - std::vector collision_points; + std::unordered_map> sources_collision_points_map; + + // By default - there is no action + Action robot_action{DO_NOTHING, cmd_vel_in, ""}; + // Polygon causing robot action (if any) + std::shared_ptr action_polygon; - // Fill collision_points array from different data sources + // Fill collision points array from different data sources + auto marker_array = std::make_unique(); for (std::shared_ptr source : sources_) { + auto iter = sources_collision_points_map.insert( + {source->getSourceName(), std::vector()}); + if (source->getEnabled()) { - source->getData(curr_time, collision_points); + if (!source->getData(curr_time, iter.first->second) && + source->getSourceTimeout().seconds() != 0.0) + { + action_polygon = nullptr; + robot_action.polygon_name = "invalid source"; + robot_action.action_type = STOP; + robot_action.req_vel.x = 0.0; + robot_action.req_vel.y = 0.0; + robot_action.req_vel.tw = 0.0; + break; + } + } + + if (collision_points_marker_pub_->get_subscription_count() > 0) { + // visualize collision points with markers + 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_" + source->getSourceName(); + 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); + marker.frame_locked = true; + + for (const auto & point : iter.first->second) { + 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); } } - // By default - there is no action - Action robot_action{DO_NOTHING, cmd_vel_in}; - // Polygon causing robot action (if any) - std::shared_ptr action_polygon; + if (collision_points_marker_pub_->get_subscription_count() > 0) { + collision_points_marker_pub_->publish(std::move(marker_array)); + } for (std::shared_ptr polygon : polygons_) { if (!polygon->getEnabled()) { @@ -376,23 +455,28 @@ 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) { + if (at == STOP || at == SLOWDOWN || at == LIMIT) { // Process STOP/SLOWDOWN for the selected polygon - if (processStopSlowdown(polygon, collision_points, cmd_vel_in, robot_action)) { + if (processStopSlowdownLimit( + polygon, sources_collision_points_map, cmd_vel_in, robot_action)) + { action_polygon = polygon; } } else if (at == APPROACH) { // Process APPROACH for the selected polygon - if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) { + if (processApproach(polygon, sources_collision_points_map, cmd_vel_in, robot_action)) { action_polygon = polygon; } } } - if (robot_action.action_type != robot_action_prev_.action_type) { + if (robot_action.polygon_name != robot_action_prev_.polygon_name) { // Report changed robot behavior - printAction(robot_action, action_polygon); + notifyActionState(robot_action, action_polygon); } // Publish required robot velocity @@ -404,29 +488,55 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) robot_action_prev_ = robot_action; } -bool CollisionMonitor::processStopSlowdown( +bool CollisionMonitor::processStopSlowdownLimit( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const { - if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) { + if (!polygon->isShapeSet()) { + return false; + } + + if (polygon->getPointsInside(sources_collision_points_map) >= polygon->getMinPoints()) { if (polygon->getActionType() == STOP) { // Setting up zero velocity for STOP model + robot_action.polygon_name = polygon->getName(); robot_action.action_type = STOP; robot_action.req_vel.x = 0.0; robot_action.req_vel.y = 0.0; robot_action.req_vel.tw = 0.0; return true; - } else { // SLOWDOWN + } else if (polygon->getActionType() == SLOWDOWN) { const Velocity safe_vel = velocity * polygon->getSlowdownRatio(); // Check that currently calculated velocity is safer than // chosen for previous shapes one if (safe_vel < robot_action.req_vel) { + robot_action.polygon_name = polygon->getName(); robot_action.action_type = SLOWDOWN; robot_action.req_vel = safe_vel; return true; } + } else { // Limit + // Compute linear velocity + const double linear_vel = std::hypot(velocity.x, velocity.y); // absolute + Velocity safe_vel; + double ratio = 1.0; + if (linear_vel != 0.0) { + ratio = std::clamp(polygon->getLinearLimit() / linear_vel, 0.0, 1.0); + } + safe_vel.x = velocity.x * ratio; + safe_vel.y = velocity.y * ratio; + safe_vel.tw = std::clamp( + velocity.tw, -polygon->getAngularLimit(), polygon->getAngularLimit()); + // Check that currently calculated velocity is safer than + // chosen for previous shapes one + if (safe_vel < robot_action.req_vel) { + robot_action.polygon_name = polygon->getName(); + robot_action.action_type = LIMIT; + robot_action.req_vel = safe_vel; + return true; + } } } @@ -435,14 +545,16 @@ bool CollisionMonitor::processStopSlowdown( bool CollisionMonitor::processApproach( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const { - polygon->updatePolygon(); + if (!polygon->isShapeSet()) { + return false; + } // Obtain time before a collision - const double collision_time = polygon->getCollisionTime(collision_points, velocity); + const double collision_time = polygon->getCollisionTime(sources_collision_points_map, velocity); if (collision_time >= 0.0) { // If collision will occurr, reduce robot speed const double change_ratio = collision_time / polygon->getTimeBeforeCollision(); @@ -450,6 +562,7 @@ bool CollisionMonitor::processApproach( // Check that currently calculated velocity is safer than // chosen for previous shapes one if (safe_vel < robot_action.req_vel) { + robot_action.polygon_name = polygon->getName(); robot_action.action_type = APPROACH; robot_action.req_vel = safe_vel; return true; @@ -459,20 +572,33 @@ bool CollisionMonitor::processApproach( return false; } -void CollisionMonitor::printAction( +void CollisionMonitor::notifyActionState( const Action & robot_action, const std::shared_ptr action_polygon) const { if (robot_action.action_type == STOP) { - RCLCPP_INFO( - get_logger(), - "Robot to stop due to %s polygon", - action_polygon->getName().c_str()); + if (robot_action.polygon_name == "invalid source") { + RCLCPP_WARN( + get_logger(), + "Robot to stop due to invalid source." + " 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"); + } else { + RCLCPP_INFO( + get_logger(), + "Robot to stop due to %s polygon", + action_polygon->getName().c_str()); + } } else if (robot_action.action_type == SLOWDOWN) { RCLCPP_INFO( get_logger(), "Robot to slowdown for %f percents due to %s polygon", action_polygon->getSlowdownRatio() * 100, action_polygon->getName().c_str()); + } else if (robot_action.action_type == LIMIT) { + RCLCPP_INFO( + get_logger(), + "Robot to limit speed due to %s polygon", + action_polygon->getName().c_str()); } else if (robot_action.action_type == APPROACH) { RCLCPP_INFO( get_logger(), @@ -483,6 +609,15 @@ void CollisionMonitor::printAction( get_logger(), "Robot to continue normal operation"); } + + if (state_pub_) { + std::unique_ptr state_msg = + std::make_unique(); + state_msg->polygon_name = robot_action.polygon_name; + state_msg->action_type = robot_action.action_type; + + state_pub_->publish(std::move(state_msg)); + } } void CollisionMonitor::publishPolygons() const diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 7e84d8890a0..77daab68055 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -17,8 +17,10 @@ #include #include "sensor_msgs/point_cloud2_iterator.hpp" +#include "tf2/transform_datatypes.h" #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_collision_monitor { @@ -64,42 +66,22 @@ void PointCloud::configure() std::bind(&PointCloud::dataCallback, this, std::placeholders::_1)); } -void PointCloud::getData( +bool PointCloud::getData( const rclcpp::Time & curr_time, - std::vector & data) const + std::vector & data) { // Ignore data from the source if it is not being published yet or // not published for a long time if (data_ == nullptr) { - return; + return false; } if (!sourceValid(data_->header.stamp, curr_time)) { - return; + return false; } tf2::Transform tf_transform; - if (base_shift_correction_) { - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time - if ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; - } - } else { - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - if ( - !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; - } + if (!getTransform(curr_time, data_->header, tf_transform)) { + return false; } sensor_msgs::PointCloud2ConstIterator iter_x(*data_, "x"); @@ -117,6 +99,7 @@ void PointCloud::getData( data.push_back({p_v3_b.x(), p_v3_b.y()}); } } + return true; } void PointCloud::getParameters(std::string & source_topic) diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index b756c98afce..19ac5053de8 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -19,8 +19,11 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/point32.hpp" +#include "tf2/transform_datatypes.h" #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/array_parser.hpp" #include "nav2_collision_monitor/kinematics.hpp" @@ -34,7 +37,8 @@ Polygon::Polygon( const std::string & base_frame_id, const tf2::Duration & transform_tolerance) : node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING), - slowdown_ratio_(0.0), footprint_sub_(nullptr), tf_buffer_(tf_buffer), + slowdown_ratio_(0.0), linear_limit_(0.0), angular_limit_(0.0), + footprint_sub_(nullptr), tf_buffer_(tf_buffer), base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance) { RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str()); @@ -43,6 +47,8 @@ Polygon::Polygon( Polygon::~Polygon() { RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str()); + polygon_sub_.reset(); + polygon_pub_.reset(); poly_.clear(); dyn_params_handler_.reset(); } @@ -54,20 +60,27 @@ bool Polygon::configure() throw std::runtime_error{"Failed to lock node"}; } - std::string polygon_pub_topic, footprint_topic; + std::string polygon_sub_topic, polygon_pub_topic, footprint_topic; - if (!getParameters(polygon_pub_topic, footprint_topic)) { + if (!getParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) { return false; } + createSubscription(polygon_sub_topic); + if (!footprint_topic.empty()) { + RCLCPP_INFO( + logger_, + "[%s]: Making footprint subscriber on %s topic", + polygon_name_.c_str(), footprint_topic.c_str()); footprint_sub_ = std::make_unique( node, footprint_topic, *tf_buffer_, base_frame_id_, tf2::durationToSec(transform_tolerance_)); } if (visualize_) { - // Fill polygon_ points for future usage + // Fill polygon_ for future usage + polygon_.header.frame_id = base_frame_id_; std::vector poly; getPolygon(poly); for (const Point & p : poly) { @@ -75,7 +88,7 @@ bool Polygon::configure() p_s.x = p.x; p_s.y = p.y; // p_s.z will remain 0.0 - polygon_.points.push_back(p_s); + polygon_.polygon.points.push_back(p_s); } rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default @@ -86,7 +99,7 @@ bool Polygon::configure() // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind(&Polygon::dynamicParametersCallback, this, std::placeholders::_1)); - + return true; } @@ -119,9 +132,9 @@ bool Polygon::getEnabled() const return enabled_; } -int Polygon::getMaxPoints() const +int Polygon::getMinPoints() const { - return max_points_; + return min_points_; } double Polygon::getSlowdownRatio() const @@ -129,17 +142,41 @@ double Polygon::getSlowdownRatio() const return slowdown_ratio_; } +double Polygon::getLinearLimit() const +{ + return linear_limit_; +} + +double Polygon::getAngularLimit() const +{ + return angular_limit_; +} + double Polygon::getTimeBeforeCollision() const { return time_before_collision_; } +std::vector Polygon::getSourcesNames() const +{ + return sources_names_; +} + 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 @@ -149,14 +186,39 @@ void Polygon::updatePolygon() std::size_t new_size = footprint_vec.size(); poly_.resize(new_size); - polygon_.points.resize(new_size); + polygon_.header.frame_id = base_frame_id_; + polygon_.polygon.points.resize(new_size); geometry_msgs::msg::Point32 p_s; for (std::size_t i = 0; i < new_size; i++) { poly_[i] = {footprint_vec[i].x, footprint_vec[i].y}; p_s.x = footprint_vec[i].x; p_s.y = footprint_vec[i].y; - polygon_.points[i] = p_s; + polygon_.polygon.points[i] = p_s; + } + } else if (!polygon_.header.frame_id.empty() && polygon_.header.frame_id != base_frame_id_) { + // Polygon is published in another frame: correct poly_ vertices to the latest frame state + std::size_t new_size = polygon_.polygon.points.size(); + + // Get the transform from PolygonStamped frame to base_frame_id_ + tf2::Stamped tf_transform; + if ( + !nav2_util::getTransform( + polygon_.header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } + + // Correct main poly_ vertices + poly_.resize(new_size); + for (std::size_t i = 0; i < new_size; i++) { + // Transform point coordinates from PolygonStamped frame -> to base frame + tf2::Vector3 p_v3_s(polygon_.polygon.points[i].x, polygon_.polygon.points[i].y, 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Fill poly_ array + poly_[i] = {p_v3_b.x(), p_v3_b.y()}; } } } @@ -172,19 +234,48 @@ int Polygon::getPointsInside(const std::vector & points) const return num; } +int Polygon::getPointsInside( + const std::unordered_map> & sources_collision_points_map) const +{ + int num = 0; + std::vector polygon_sources_names = getSourcesNames(); + + // Sum the number of points from all sources associated with current polygon + for (const auto & source_name : polygon_sources_names) { + const auto & iter = sources_collision_points_map.find(source_name); + if (iter != sources_collision_points_map.end()) { + num += getPointsInside(iter->second); + } + } + + return num; +} + double Polygon::getCollisionTime( - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity) const { // Initial robot pose is {0,0} in base_footprint coordinates Pose pose = {0.0, 0.0, 0.0}; Velocity vel = velocity; + std::vector polygon_sources_names = getSourcesNames(); + std::vector collision_points; + + // Save all points coming from the sources associated with current polygon + for (const auto & source_name : polygon_sources_names) { + const auto & iter = sources_collision_points_map.find(source_name); + if (iter != sources_collision_points_map.end()) { + collision_points.insert(collision_points.end(), iter->second.begin(), iter->second.end()); + } + } + // Array of points transformed to the frame concerned with pose on each simulation step std::vector points_transformed = collision_points; // Check static polygon - if (getPointsInside(points_transformed) >= max_points_) { + if (getPointsInside(collision_points) >= min_points_) { return 0.0; } @@ -198,7 +289,7 @@ double Polygon::getCollisionTime( transformPoints(pose, points_transformed); // If the collision occurred on this stage, return the actual time before a collision // as if robot was moved with given velocity - if (getPointsInside(points_transformed) > max_points_) { + if (getPointsInside(points_transformed) >= min_points_) { return time; } } @@ -207,7 +298,7 @@ double Polygon::getCollisionTime( return -1.0; } -void Polygon::publish() const +void Polygon::publish() { if (!visualize_) { return; @@ -218,18 +309,17 @@ void Polygon::publish() const throw std::runtime_error{"Failed to lock node"}; } - // Fill PolygonStamped struct - std::unique_ptr poly_s = - std::make_unique(); - poly_s->header.stamp = node->now(); - poly_s->header.frame_id = base_frame_id_; - poly_s->polygon = polygon_; - - // Publish polygon - polygon_pub_->publish(std::move(poly_s)); + // Actualize the time to current and publish the polygon + polygon_.header.stamp = node->now(); + auto msg = std::make_unique(polygon_); + polygon_pub_->publish(std::move(msg)); } -bool Polygon::getCommonParameters(std::string & polygon_pub_topic) +bool Polygon::getCommonParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic, + bool use_dynamic_sub_topic) { auto node = node_.lock(); if (!node) { @@ -247,6 +337,8 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) action_type_ = STOP; } else if (at_str == "slowdown") { action_type_ = SLOWDOWN; + } else if (at_str == "limit") { + action_type_ = LIMIT; } else if (at_str == "approach") { action_type_ = APPROACH; } else if (at_str == "none") { @@ -259,10 +351,23 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".enabled", rclcpp::ParameterValue(true)); enabled_ = node->get_parameter(polygon_name_ + ".enabled").as_bool(); - + nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".max_points", rclcpp::ParameterValue(3)); - max_points_ = node->get_parameter(polygon_name_ + ".max_points").as_int(); + node, polygon_name_ + ".min_points", rclcpp::ParameterValue(4)); + min_points_ = node->get_parameter(polygon_name_ + ".min_points").as_int(); + + try { + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".max_points", rclcpp::PARAMETER_INTEGER); + min_points_ = node->get_parameter(polygon_name_ + ".max_points").as_int() + 1; + RCLCPP_WARN( + logger_, + "[%s]: \"max_points\" parameter was deprecated. Use \"min_points\" instead to specify " + "the minimum number of data readings within a zone to trigger the action", + polygon_name_.c_str()); + } catch (const std::exception &) { + // This is normal situation: max_points parameter should not being declared + } if (action_type_ == SLOWDOWN) { nav2_util::declare_parameter_if_not_declared( @@ -270,6 +375,15 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) slowdown_ratio_ = node->get_parameter(polygon_name_ + ".slowdown_ratio").as_double(); } + if (action_type_ == LIMIT) { + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".linear_limit", rclcpp::ParameterValue(0.5)); + linear_limit_ = node->get_parameter(polygon_name_ + ".linear_limit").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".angular_limit", rclcpp::ParameterValue(0.5)); + angular_limit_ = node->get_parameter(polygon_name_ + ".angular_limit").as_double(); + } + if (action_type_ == APPROACH) { nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".time_before_collision", rclcpp::ParameterValue(2.0)); @@ -290,6 +404,51 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) node, polygon_name_ + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name_)); polygon_pub_topic = node->get_parameter(polygon_name_ + ".polygon_pub_topic").as_string(); } + + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".polygon_subscribe_transient_local", rclcpp::ParameterValue(false)); + polygon_subscribe_transient_local_ = + node->get_parameter(polygon_name_ + ".polygon_subscribe_transient_local").as_bool(); + + if (use_dynamic_sub_topic) { + if (action_type_ != APPROACH) { + // Get polygon sub topic + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING); + polygon_sub_topic = + node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string(); + } else { + // Obtain the footprint topic to make a footprint subscription for approach polygon + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".footprint_topic", + rclcpp::ParameterValue("local_costmap/published_footprint")); + footprint_topic = + node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); + } + } + + // By default, use all observation sources for polygon + nav2_util::declare_parameter_if_not_declared( + node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY); + const std::vector observation_sources = + node->get_parameter("observation_sources").as_string_array(); + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".sources_names", rclcpp::ParameterValue(observation_sources)); + sources_names_ = node->get_parameter(polygon_name_ + ".sources_names").as_string_array(); + + // Check the observation sources configured for polygon are defined + for (auto source_name : sources_names_) { + if (std::find(observation_sources.begin(), observation_sources.end(), source_name) == + observation_sources.end()) + { + RCLCPP_ERROR_STREAM( + logger_, + "Observation source [" << source_name << + "] configured for polygon [" << getName() << + "] is not defined as one of the node's observation_source!"); + return false; + } + } } catch (const std::exception & ex) { RCLCPP_ERROR( logger_, @@ -301,69 +460,110 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) return true; } -bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) +bool Polygon::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"}; } - if (!getCommonParameters(polygon_pub_topic)) { - return false; - } + // Clear the subscription topics. They will be set later, if necessary. + polygon_sub_topic.clear(); + footprint_topic.clear(); + bool use_dynamic_sub = true; // if getting parameter points fails, use dynamic subscription try { - if (action_type_ == APPROACH) { - // Obtain the footprint topic to make a footprint subscription for approach polygon - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".footprint_topic", - rclcpp::ParameterValue("local_costmap/published_footprint")); - footprint_topic = - node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); - - // This is robot footprint: do not need to get polygon points from ROS parameters. - // It will be set dynamically later. - return true; - } else { - // Make it empty otherwise - footprint_topic.clear(); - } - - // Leave it not initialized: the will cause an error if it will not set + // Leave it uninitialized: it will throw an inner exception if the parameter is not set nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); - std::vector poly_row = - node->get_parameter(polygon_name_ + ".points").as_double_array(); - // Check for points format correctness - if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { + node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING); + std::string poly_string = + node->get_parameter(polygon_name_ + ".points").as_string(); + + use_dynamic_sub = !getPolygonFromString(poly_string, poly_); + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { + RCLCPP_INFO( + logger_, + "[%s]: Polygon points are not defined. Using dynamic subscription instead.", + polygon_name_.c_str()); + } + + if (!getCommonParameters( + polygon_sub_topic, polygon_pub_topic, footprint_topic, use_dynamic_sub)) + { + if (use_dynamic_sub && polygon_sub_topic.empty() && footprint_topic.empty()) { RCLCPP_ERROR( logger_, - "[%s]: Polygon has incorrect points description", + "[%s]: Error while getting polygon parameters:" + " static points and sub topic both not defined", polygon_name_.c_str()); - return false; } + return false; + } - // Obtain polygon vertices - Point point; - bool first = true; - for (double val : poly_row) { - if (first) { - point.x = val; - } else { - point.y = val; - poly_.push_back(point); - } - first = !first; + return true; +} + +void Polygon::createSubscription(std::string & polygon_sub_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!polygon_sub_topic.empty()) { + RCLCPP_INFO( + logger_, + "[%s]: Subscribing on %s topic for polygon", + polygon_name_.c_str(), polygon_sub_topic.c_str()); + rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + if (polygon_subscribe_transient_local_) { + polygon_qos.transient_local(); } - } catch (const std::exception & ex) { + polygon_sub_ = node->create_subscription( + polygon_sub_topic, polygon_qos, + std::bind(&Polygon::polygonCallback, this, std::placeholders::_1)); + } +} + +void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) +{ + std::size_t new_size = msg->polygon.points.size(); + + if (new_size < 3) { RCLCPP_ERROR( logger_, - "[%s]: Error while getting polygon parameters: %s", - polygon_name_.c_str(), ex.what()); - return false; + "[%s]: Polygon should have at least 3 points", + polygon_name_.c_str()); + return; } - return true; + // Get the transform from PolygonStamped frame to base_frame_id_ + tf2::Stamped tf_transform; + if ( + !nav2_util::getTransform( + msg->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } + + // Set main poly_ vertices first time + poly_.resize(new_size); + for (std::size_t i = 0; i < new_size; i++) { + // Transform point coordinates from PolygonStamped frame -> to base frame + tf2::Vector3 p_v3_s(msg->polygon.points[i].x, msg->polygon.points[i].y, 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Fill poly_ array + poly_[i] = {p_v3_b.x(), p_v3_b.y()}; + } + + // Store incoming polygon for further (possible) poly_ vertices corrections + // from PolygonStamped frame -> to base frame + polygon_ = *msg; } rcl_interfaces::msg::SetParametersResult @@ -386,6 +586,15 @@ Polygon::dynamicParametersCallback( return result; } +void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) +{ + RCLCPP_INFO( + logger_, + "[%s]: Polygon shape update has been arrived", + polygon_name_.c_str()); + updatePolygon(msg); +} + inline bool Polygon::isPointInside(const Point & point) const { // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." @@ -418,4 +627,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/polygon_source.cpp b/nav2_collision_monitor/src/polygon_source.cpp new file mode 100644 index 00000000000..6e7ffa3572d --- /dev/null +++ b/nav2_collision_monitor/src/polygon_source.cpp @@ -0,0 +1,186 @@ +// 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/polygon_source.hpp" + +#include +#include + +#include "geometry_msgs/msg/polygon_stamped.hpp" +#include "tf2/transform_datatypes.h" + +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" + + +namespace nav2_collision_monitor +{ + +PolygonSource::PolygonSource( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) +: Source( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, source_timeout, base_shift_correction) +{ +} + +PolygonSource::~PolygonSource() +{ + data_sub_.reset(); +} + +void PolygonSource::configure() +{ + Source::configure(); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + std::string source_topic; + + getParameters(source_topic); + + rclcpp::QoS qos = rclcpp::SensorDataQoS(); // set to default + data_sub_ = node->create_subscription( + source_topic, qos, + std::bind(&PolygonSource::dataCallback, this, std::placeholders::_1)); +} + +bool PolygonSource::getData( + const rclcpp::Time & curr_time, + std::vector & data) +{ + // Ignore data from the source if it is not being published yet or + // not published for a long time + if (data_.empty()) { + return false; + } + + // Remove stale data + data_.erase( + std::remove_if( + data_.begin(), data_.end(), + [this, curr_time](const nav2_msgs::msg::PolygonInstanceStamped & polygon_stamped) { + return curr_time - rclcpp::Time(polygon_stamped.header.stamp) > source_timeout_; + }), data_.end()); + + tf2::Stamped tf_transform; + for (const auto & polygon_instance : data_) { + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + polygon_instance.header.frame_id, polygon_instance.header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + if ( + !nav2_util::getTransform( + polygon_instance.header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } + geometry_msgs::msg::PolygonStamped poly_out, polygon_stamped; + geometry_msgs::msg::TransformStamped tf = tf2::toMsg(tf_transform); + polygon_stamped.header = polygon_instance.header; + polygon_stamped.polygon = polygon_instance.polygon.polygon; + tf2::doTransform(polygon_stamped, poly_out, tf); + convertPolygonStampedToPoints(poly_out, data); + } + return true; +} + +void PolygonSource::convertPolygonStampedToPoints( + const geometry_msgs::msg::PolygonStamped & polygon, std::vector & data) const +{ + /* Full function generated by GPT */ + + // Iterate over the vertices of the polygon + for (size_t i = 0; i < polygon.polygon.points.size(); ++i) { + const auto & current_point = polygon.polygon.points[i]; + const auto & next_point = + polygon.polygon.points[(i + 1) % polygon.polygon.points.size()]; + + // Calculate the distance between the current and next points + double segment_length = + std::hypot(next_point.x - current_point.x, next_point.y - current_point.y); + + // Calculate the number of points to sample in the current segment + int num_points_in_segment = + std::max(static_cast(std::ceil(segment_length / sampling_distance_)), 1); + + // Calculate the step size for each pair of vertices + const double dx = (next_point.x - current_point.x) / num_points_in_segment; + const double dy = (next_point.y - current_point.y) / num_points_in_segment; + + // Sample the points with equal spacing + for (int j = 0; j <= num_points_in_segment; ++j) { + Point p; + p.x = current_point.x + j * dx; + p.y = current_point.y + j * dy; + data.push_back(p); + } + } +} + +void PolygonSource::getParameters(std::string & source_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + getCommonParameters(source_topic); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".sampling_distance", rclcpp::ParameterValue(0.1)); + sampling_distance_ = node->get_parameter(source_name_ + ".sampling_distance").as_double(); +} + +void PolygonSource::dataCallback(nav2_msgs::msg::PolygonInstanceStamped::ConstSharedPtr msg) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + auto curr_time = node->now(); + + // check if older similar polygon exists already and replace it with the new one + for (auto & polygon_stamped : data_) { + if (msg->polygon.id == polygon_stamped.polygon.id) { + polygon_stamped = *msg; + return; + } + } + data_.push_back(*msg); +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 8f750490962..fe555b171f3 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -18,7 +18,10 @@ #include #include +#include "tf2/transform_datatypes.h" + #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_collision_monitor { @@ -64,17 +67,17 @@ void Range::configure() std::bind(&Range::dataCallback, this, std::placeholders::_1)); } -void Range::getData( +bool Range::getData( const rclcpp::Time & curr_time, - std::vector & data) const + std::vector & data) { // Ignore data from the source if it is not being published yet or // not being published for a long time if (data_ == nullptr) { - return; + return false; } if (!sourceValid(data_->header.stamp, curr_time)) { - return; + return false; } // Ignore data, if its range is out of scope of range sensor abilities @@ -83,32 +86,12 @@ void Range::getData( logger_, "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...", source_name_.c_str(), data_->range, data_->min_range, data_->max_range); - return; + return false; } tf2::Transform tf_transform; - if (base_shift_correction_) { - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time - if ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; - } - } else { - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - if ( - !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; - } + if (!getTransform(curr_time, data_->header, tf_transform)) { + return false; } // Calculate poses and refill data array @@ -141,6 +124,8 @@ void Range::getData( // Refill data array data.push_back({p_v3_b.x(), p_v3_b.y()}); + + return true; } void Range::getParameters(std::string & source_topic) diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 8ac45904a21..f891df69a0a 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -17,6 +17,10 @@ #include #include +#include "tf2/transform_datatypes.h" + +#include "nav2_util/robot_utils.hpp" + namespace nav2_collision_monitor { @@ -62,42 +66,22 @@ void Scan::configure() std::bind(&Scan::dataCallback, this, std::placeholders::_1)); } -void Scan::getData( +bool Scan::getData( const rclcpp::Time & curr_time, - std::vector & data) const + std::vector & data) { // Ignore data from the source if it is not being published yet or // not being published for a long time if (data_ == nullptr) { - return; + return false; } if (!sourceValid(data_->header.stamp, curr_time)) { - return; + return false; } tf2::Transform tf_transform; - if (base_shift_correction_) { - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time - if ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; - } - } else { - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - if ( - !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; - } + if (!getTransform(curr_time, data_->header, tf_transform)) { + return false; } // Calculate poses and refill data array @@ -116,6 +100,7 @@ void Scan::getData( } angle += data_->angle_increment; } + return true; } void Scan::dataCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg) diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index 41b6f647ee8..c7aafd06ace 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -18,10 +18,8 @@ #include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2/convert.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_collision_monitor { @@ -72,6 +70,12 @@ void Source::getCommonParameters(std::string & source_topic) nav2_util::declare_parameter_if_not_declared( node, source_name_ + ".enabled", rclcpp::ParameterValue(true)); enabled_ = node->get_parameter(source_name_ + ".enabled").as_bool(); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".source_timeout", + rclcpp::ParameterValue(source_timeout_.seconds())); // node source_timeout by default + source_timeout_ = rclcpp::Duration::from_seconds( + node->get_parameter(source_name_ + ".source_timeout").as_double()); } bool Source::sourceValid( @@ -81,7 +85,7 @@ bool Source::sourceValid( // Source is considered as not valid, if latest received data timestamp is earlier // than current time by source_timeout_ interval const rclcpp::Duration dt = curr_time - source_time; - if (dt > source_timeout_) { + if (source_timeout_.seconds() != 0.0 && dt > source_timeout_) { RCLCPP_WARN( logger_, "[%s]: Latest source and current collision monitor node timestamps differ on %f seconds. " @@ -98,6 +102,16 @@ bool Source::getEnabled() const return enabled_; } +std::string Source::getSourceName() const +{ + return source_name_; +} + +rclcpp::Duration Source::getSourceTimeout() const +{ + return source_timeout_; +} + rcl_interfaces::msg::SetParametersResult Source::dynamicParametersCallback( std::vector parameters) @@ -118,4 +132,30 @@ Source::dynamicParametersCallback( return result; } +bool Source::getTransform( + const rclcpp::Time & curr_time, + const std_msgs::msg::Header & data_header, + tf2::Transform & tf_transform) const +{ + if (base_shift_correction_) { + if ( + !nav2_util::getTransform( + data_header.frame_id, data_header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } else { + if ( + !nav2_util::getTransform( + data_header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + 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 00000000000..602a018c2bb --- /dev/null +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -0,0 +1,193 @@ +// 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_sub_topic, polygon_pub_topic, footprint_topic, false)) { + 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 new file mode 100644 index 00000000000..2f879e1d396 --- /dev/null +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -0,0 +1,52 @@ +# Kinematics test +ament_add_gtest(kinematics_test kinematics_test.cpp) +ament_target_dependencies(kinematics_test + ${dependencies} +) +target_link_libraries(kinematics_test + ${monitor_library_name} +) + +# Data sources test +ament_add_gtest(sources_test sources_test.cpp) +ament_target_dependencies(sources_test + ${dependencies} +) +target_link_libraries(sources_test + ${monitor_library_name} +) + +# Polygon shapes test +ament_add_gtest(polygons_test polygons_test.cpp) +ament_target_dependencies(polygons_test + ${dependencies} +) +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 + ${dependencies} +) +target_link_libraries(collision_monitor_node_test + ${monitor_library_name} +) +# Collision Detector node test +ament_add_gtest(collision_detector_node_test collision_detector_node_test.cpp) +ament_target_dependencies(collision_detector_node_test + ${dependencies} +) +target_link_libraries(collision_detector_node_test + ${detector_library_name} +) \ No newline at end of file diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp new file mode 100644 index 00000000000..36a1a266c41 --- /dev/null +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -0,0 +1,833 @@ +// 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 + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/range.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "tf2_ros/transform_broadcaster.h" + +#include "nav2_collision_monitor/types.hpp" +#include "nav2_collision_monitor/collision_detector_node.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = 1e-5; + +static const char BASE_FRAME_ID[]{"base_link"}; +static const char SOURCE_FRAME_ID[]{"base_source"}; +static const char ODOM_FRAME_ID[]{"odom"}; +static const char SCAN_NAME[]{"Scan"}; +static const char POINTCLOUD_NAME[]{"PointCloud"}; +static const char RANGE_NAME[]{"Range"}; +static const char POLYGON_NAME[]{"Polygon"}; +static const char STATE_TOPIC[]{"collision_detector_state"}; +static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_detector/collision_points_marker"}; +static const int MIN_POINTS{1}; +static const double SIMULATION_TIME_STEP{0.01}; +static const double TRANSFORM_TOLERANCE{0.5}; +static const double SOURCE_TIMEOUT{5.0}; +static const double FREQUENCY{10.0}; + +enum PolygonType +{ + POLYGON_UNKNOWN = 0, + POLYGON = 1, + CIRCLE = 2 +}; + +enum SourceType +{ + SOURCE_UNKNOWN = 0, + SCAN = 1, + POINTCLOUD = 2, + RANGE = 3, + POLYGON_SOURCE = 4 +}; + +class CollisionDetectorWrapper : public nav2_collision_monitor::CollisionDetector +{ +public: + void start() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_activate(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void stop() + { + ASSERT_EQ(on_deactivate(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_cleanup(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void configure() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void cant_configure() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::FAILURE); + } + + bool correctDataReceived(const double expected_dist, const rclcpp::Time & stamp) + { + for (std::shared_ptr source : sources_) { + std::vector collision_points; + source->getData(stamp, collision_points); + if (collision_points.size() != 0) { + const double dist = std::hypot(collision_points[0].x, collision_points[0].y); + if (std::fabs(dist - expected_dist) <= EPSILON) { + return true; + } + } + } + return false; + } +}; // CollisionDetectorWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + + // Configuring + void setCommonParameters(); + void addPolygon( + const std::string & polygon_name, const PolygonType type, + const double size, const std::string & at); + void addSource(const std::string & source_name, const SourceType type); + void setVectors( + const std::vector & polygons, + const std::vector & sources); + + // Setting TF chains + void sendTransforms(const rclcpp::Time & stamp); + + // Publish robot footprint + void publishFootprint(const double radius, const rclcpp::Time & stamp); + + // Main topic/data working routines + void publishScan(const double dist, const rclcpp::Time & stamp); + void publishPointCloud(const double dist, const rclcpp::Time & stamp); + void publishRange(const double dist, const rclcpp::Time & stamp); + void publishPolygon(const double dist, const rclcpp::Time & stamp); + bool waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp); + bool waitState(const std::chrono::nanoseconds & timeout); + void stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg); + bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout); + void collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg); + +protected: + // CollisionDetector node + std::shared_ptr cd_; + + // Data source publishers + rclcpp::Publisher::SharedPtr scan_pub_; + rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr range_pub_; + rclcpp::Publisher::SharedPtr polygon_source_pub_; + + rclcpp::Subscription::SharedPtr state_sub_; + nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_; + + // CollisionMonitor collision points markers + rclcpp::Subscription::SharedPtr + collision_points_marker_sub_; + visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; +}; // Tester + +Tester::Tester() +{ + cd_ = std::make_shared(); + + scan_pub_ = cd_->create_publisher( + SCAN_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + pointcloud_pub_ = cd_->create_publisher( + POINTCLOUD_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + range_pub_ = cd_->create_publisher( + RANGE_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + polygon_source_pub_ = cd_->create_publisher( + POLYGON_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + state_sub_ = cd_->create_subscription( + STATE_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::stateCallback, this, std::placeholders::_1)); + + collision_points_marker_sub_ = cd_->create_subscription( + COLLISION_POINTS_MARKERS_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::collisionPointsMarkerCallback, this, std::placeholders::_1)); +} + +Tester::~Tester() +{ + scan_pub_.reset(); + pointcloud_pub_.reset(); + range_pub_.reset(); + polygon_source_pub_.reset(); + collision_points_marker_sub_.reset(); + + cd_.reset(); +} + +bool Tester::waitState(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cd_->now(); + while (rclcpp::ok() && cd_->now() - start_time <= rclcpp::Duration(timeout)) { + if (state_msg_) { + return true; + } + rclcpp::spin_some(cd_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout) +{ + collision_points_marker_msg_ = nullptr; + rclcpp::Time start_time = cd_->now(); + while (rclcpp::ok() && cd_->now() - start_time <= rclcpp::Duration(timeout)) { + if (collision_points_marker_msg_) { + return true; + } + rclcpp::spin_some(cd_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +void Tester::stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg) +{ + state_msg_ = msg; +} + +void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg) +{ + collision_points_marker_msg_ = msg; +} + +void Tester::setCommonParameters() +{ + cd_->declare_parameter( + "base_frame_id", rclcpp::ParameterValue(BASE_FRAME_ID)); + cd_->set_parameter( + rclcpp::Parameter("base_frame_id", BASE_FRAME_ID)); + cd_->declare_parameter( + "odom_frame_id", rclcpp::ParameterValue(ODOM_FRAME_ID)); + cd_->set_parameter( + rclcpp::Parameter("odom_frame_id", ODOM_FRAME_ID)); + + cd_->declare_parameter( + "transform_tolerance", rclcpp::ParameterValue(TRANSFORM_TOLERANCE)); + cd_->set_parameter( + rclcpp::Parameter("transform_tolerance", TRANSFORM_TOLERANCE)); + cd_->declare_parameter( + "source_timeout", rclcpp::ParameterValue(SOURCE_TIMEOUT)); + cd_->set_parameter( + rclcpp::Parameter("source_timeout", SOURCE_TIMEOUT)); + cd_->declare_parameter( + "frequency", rclcpp::ParameterValue(FREQUENCY)); + cd_->set_parameter( + rclcpp::Parameter("frequency", FREQUENCY)); +} + +void Tester::addPolygon( + const std::string & polygon_name, const PolygonType type, + const double size, const std::string & at) +{ + if (type == POLYGON) { + cd_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + 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) + "]]"; + cd_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(points)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", points)); + } else if (type == CIRCLE) { + cd_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("circle")); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "circle")); + + cd_->declare_parameter( + polygon_name + ".radius", rclcpp::ParameterValue(size)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".radius", size)); + } else { // type == POLYGON_UNKNOWN + cd_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("unknown")); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "unknown")); + } + + cd_->declare_parameter( + polygon_name + ".action_type", rclcpp::ParameterValue(at)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".action_type", at)); + + cd_->declare_parameter( + polygon_name + ".min_points", rclcpp::ParameterValue(MIN_POINTS)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".min_points", MIN_POINTS)); + + cd_->declare_parameter( + polygon_name + ".simulation_time_step", rclcpp::ParameterValue(SIMULATION_TIME_STEP)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".simulation_time_step", SIMULATION_TIME_STEP)); + + cd_->declare_parameter( + polygon_name + ".visualize", rclcpp::ParameterValue(false)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".visualize", false)); + + cd_->declare_parameter( + polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".polygon_pub_topic", polygon_name)); +} + +void Tester::addSource( + const std::string & source_name, const SourceType type) +{ + if (type == SCAN) { + cd_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("scan")); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".type", "scan")); + } else if (type == POINTCLOUD) { + cd_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("pointcloud")); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".type", "pointcloud")); + + cd_->declare_parameter( + source_name + ".min_height", rclcpp::ParameterValue(0.1)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".min_height", 0.1)); + cd_->declare_parameter( + source_name + ".max_height", rclcpp::ParameterValue(1.0)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".max_height", 1.0)); + } else if (type == RANGE) { + cd_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("range")); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".type", "range")); + + cd_->declare_parameter( + source_name + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 200)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".obstacles_angle", M_PI / 200)); + } else if (type == POLYGON_SOURCE) { + cd_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("polygon")); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".type", "polygon")); + + cd_->declare_parameter( + source_name + ".sampling_distance", rclcpp::ParameterValue(0.1)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".sampling_distance", 0.1)); + cd_->declare_parameter( + source_name + ".polygon_similarity_threshold", rclcpp::ParameterValue(2.0)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".polygon_similarity_threshold", 2.0)); + } else { // type == SOURCE_UNKNOWN + cd_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("unknown")); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".type", "unknown")); + } + + cd_->declare_parameter( + source_name + ".topic", rclcpp::ParameterValue(source_name)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".topic", source_name)); +} + +void Tester::setVectors( + const std::vector & polygons, + const std::vector & sources) +{ + cd_->declare_parameter("polygons", rclcpp::ParameterValue(polygons)); + cd_->set_parameter(rclcpp::Parameter("polygons", polygons)); + + cd_->declare_parameter("observation_sources", rclcpp::ParameterValue(sources)); + cd_->set_parameter(rclcpp::Parameter("observation_sources", sources)); +} + +void Tester::sendTransforms(const rclcpp::Time & stamp) +{ + std::shared_ptr tf_broadcaster = + std::make_shared(cd_); + + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + // Fill TF buffer ahead for future transform usage in CollisionDetector::process() + const rclcpp::Duration ahead = 1000ms; + for (rclcpp::Time t = stamp; t <= stamp + ahead; t += rclcpp::Duration(50ms)) { + transform.header.stamp = t; + + // base_frame -> source_frame transform + transform.header.frame_id = BASE_FRAME_ID; + transform.child_frame_id = SOURCE_FRAME_ID; + tf_broadcaster->sendTransform(transform); + + // odom_frame -> base_frame transform + transform.header.frame_id = ODOM_FRAME_ID; + transform.child_frame_id = BASE_FRAME_ID; + tf_broadcaster->sendTransform(transform); + } +} + +void Tester::publishScan(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->angle_min = 0.0; + msg->angle_max = 2 * M_PI; + msg->angle_increment = M_PI / 180; + msg->time_increment = 0.0; + msg->scan_time = 0.0; + msg->range_min = 0.1; + msg->range_max = dist + 1.0; + std::vector ranges(360, dist); + msg->ranges = ranges; + + scan_pub_->publish(std::move(msg)); +} + +void Tester::publishPointCloud(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*msg); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + modifier.resize(2); + + sensor_msgs::PointCloud2Iterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); + + // Point 0: (dist, 0.01, 0.2) + *iter_x = dist; + *iter_y = 0.01; + *iter_z = 0.2; + ++iter_x; ++iter_y; ++iter_z; + + // Point 1: (dist, -0.01, 0.2) + *iter_x = dist; + *iter_y = -0.01; + *iter_z = 0.2; + + pointcloud_pub_->publish(std::move(msg)); +} + +void Tester::publishRange(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->radiation_type = 0; + msg->field_of_view = M_PI / 10; + msg->min_range = 0.1; + msg->max_range = dist + 0.1; + msg->range = dist; + + range_pub_->publish(std::move(msg)); +} + +void Tester::publishPolygon(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + p.x = 1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + msg->polygon.id = 1u; + + polygon_source_pub_->publish(std::move(msg)); +} + +bool Tester::waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp) +{ + rclcpp::Time start_time = cd_->now(); + while (rclcpp::ok() && cd_->now() - start_time <= rclcpp::Duration(timeout)) { + if (cd_->correctDataReceived(expected_dist, stamp)) { + return true; + } + rclcpp::spin_some(cd_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + + +TEST_F(Tester, testIncorrectPolygonType) +{ + setCommonParameters(); + addPolygon("UnknownShape", POLYGON_UNKNOWN, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + setVectors({"UnknownShape"}, {SCAN_NAME}); + + // Check that Collision Detector node can not be configured for this parameters set + cd_->cant_configure(); +} + +TEST_F(Tester, testIncorrectActionType) +{ + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "approach"); + addSource(SCAN_NAME, SCAN); + setVectors({"DetectionRegion"}, {SCAN_NAME}); + + // Check that Collision Detector node can not be configured for this action type + cd_->cant_configure(); +} + +TEST_F(Tester, testIncorrectSourceType) +{ + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource("UnknownSource", SOURCE_UNKNOWN); + setVectors({"DetectionRegion"}, {"UnknownSource"}); + + // Check that Collision Detector node can not be configured for this parameters set + cd_->cant_configure(); +} + +TEST_F(Tester, testPolygonsNotSet) +{ + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + + // Check that Collision Detector node can not be configured for this parameters set + cd_->cant_configure(); +} + +TEST_F(Tester, testSourcesNotSet) +{ + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + cd_->declare_parameter( + "polygons", + rclcpp::ParameterValue(std::vector{"DetectionRegion"})); + cd_->set_parameter(rclcpp::Parameter("polygons", std::vector{"DetectionRegion"})); + + // Check that Collision Detector node can not be configured for this parameters set + cd_->cant_configure(); +} + +TEST_F(Tester, testSuccessfulConfigure) +{ + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + setVectors({"DetectionRegion"}, {SCAN_NAME}); + + // Check that Collision Detector node can be configured successfully for this parameters set + cd_->configure(); +} + +TEST_F(Tester, testProcessNonActive) +{ + rclcpp::Time curr_time = cd_->now(); + + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + setVectors({"DetectionRegion"}, {SCAN_NAME}); + + // Configure Collision Detector node, but not activate + cd_->configure(); + + // ... and check that the detector state was not published + ASSERT_FALSE(waitState(300ms)); + + // Stop Collision Detector node + cd_->stop(); +} + +TEST_F(Tester, testProcessActive) +{ + rclcpp::Time curr_time = cd_->now(); + + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + setVectors({"DetectionRegion"}, {SCAN_NAME}); + + // Configure and activate Collision Detector node + cd_->start(); + // ... and check that state is published + ASSERT_TRUE(waitState(300ms)); + + // Stop Collision Detector node + cd_->stop(); +} + +TEST_F(Tester, testPolygonDetection) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Detector parameters. + setCommonParameters(); + // Create polygon + addPolygon("DetectionRegion", POLYGON, 2.0, "none"); + addSource(RANGE_NAME, RANGE); + setVectors({"DetectionRegion"}, {RANGE_NAME}); + + // Start Collision Detector node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is in DetectionRegion + publishRange(1.5, curr_time); + + ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); + ASSERT_TRUE(waitState(300ms)); + ASSERT_NE(state_msg_->detections.size(), 0u); + ASSERT_EQ(state_msg_->detections[0], true); + + // Stop Collision Detector node + cd_->stop(); +} + +TEST_F(Tester, testCircleDetection) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Detector parameters. + setCommonParameters(); + // Create Circle + addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addSource(RANGE_NAME, RANGE); + setVectors({"DetectionRegion"}, {RANGE_NAME}); + + // Start Collision Detector node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is in DetectionRegion + publishRange(1.5, curr_time); + + ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); + ASSERT_TRUE(waitState(300ms)); + ASSERT_NE(state_msg_->detections.size(), 0u); + ASSERT_EQ(state_msg_->detections[0], true); + + // Stop Collision Detector node + cd_->stop(); +} + +TEST_F(Tester, testScanDetection) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Detector parameters. + setCommonParameters(); + // Create polygon + addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addSource(SCAN_NAME, SCAN); + setVectors({"DetectionRegion"}, {SCAN_NAME}); + + // Start Collision Detector node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is in DetectionRegion + publishScan(1.5, curr_time); + + ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); + ASSERT_TRUE(waitState(300ms)); + ASSERT_NE(state_msg_->detections.size(), 0u); + ASSERT_EQ(state_msg_->detections[0], true); + + // Stop Collision Detector node + cd_->stop(); +} + +TEST_F(Tester, testPointcloudDetection) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Detector parameters. + setCommonParameters(); + // Create polygon + addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addSource(POINTCLOUD_NAME, POINTCLOUD); + setVectors({"DetectionRegion"}, {POINTCLOUD_NAME}); + + // Start Collision Detector node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is in DetectionRegion + publishPointCloud(2.5, curr_time); + + ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); + ASSERT_TRUE(waitState(300ms)); + ASSERT_NE(state_msg_->detections.size(), 0u); + ASSERT_EQ(state_msg_->detections[0], true); + + // Stop Collision Detector node + cd_->stop(); +} + +TEST_F(Tester, testPolygonSourceDetection) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Detector parameters. + setCommonParameters(); + // Create polygon + addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addSource(POLYGON_NAME, POLYGON_SOURCE); + setVectors({"DetectionRegion"}, {POLYGON_NAME}); + + // Start Collision Detector node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is in DetectionRegion + publishPolygon(2.5, curr_time); + + ASSERT_TRUE(waitData(std::hypot(2.5, 1.0), 500ms, curr_time)); + ASSERT_TRUE(waitState(300ms)); + ASSERT_NE(state_msg_->detections.size(), 0u); + ASSERT_EQ(state_msg_->detections[0], true); + + // Stop Collision Detector node + cd_->stop(); +} + +TEST_F(Tester, testCollisionPointsMarkers) +{ + rclcpp::Time curr_time = cd_->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 + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); + + 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 + cd_->stop(); +} + +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; +} diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp new file mode 100644 index 00000000000..36c16dfe5ed --- /dev/null +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -0,0 +1,1648 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_msgs/msg/collision_monitor_state.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/range.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "tf2_ros/transform_broadcaster.h" + +#include "nav2_collision_monitor/types.hpp" +#include "nav2_collision_monitor/collision_monitor_node.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = 1e-5; + +static const char BASE_FRAME_ID[]{"base_link"}; +static const char SOURCE_FRAME_ID[]{"base_source"}; +static const char ODOM_FRAME_ID[]{"odom"}; +static const char CMD_VEL_IN_TOPIC[]{"cmd_vel_in"}; +static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"}; +static const char STATE_TOPIC[]{"collision_monitor_state"}; +static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_monitor/collision_points_marker"}; +static const char FOOTPRINT_TOPIC[]{"footprint"}; +static const char SCAN_NAME[]{"Scan"}; +static const char POINTCLOUD_NAME[]{"PointCloud"}; +static const char RANGE_NAME[]{"Range"}; +static const char POLYGON_NAME[]{"Polygon"}; +static const int MIN_POINTS{2}; +static const double SLOWDOWN_RATIO{0.7}; +static const double LINEAR_LIMIT{0.4}; +static const double ANGULAR_LIMIT{0.09}; +static const double TIME_BEFORE_COLLISION{1.0}; +static const double SIMULATION_TIME_STEP{0.01}; +static const double TRANSFORM_TOLERANCE{0.5}; +static const double SOURCE_TIMEOUT{5.0}; +static const double STOP_PUB_TIMEOUT{0.1}; + +enum PolygonType +{ + POLYGON_UNKNOWN = 0, + POLYGON = 1, + CIRCLE = 2, + VELOCITY_POLYGON = 3 +}; + +enum SourceType +{ + SOURCE_UNKNOWN = 0, + SCAN = 1, + POINTCLOUD = 2, + RANGE = 3, + POLYGON_SOURCE = 4 +}; + +enum ActionType +{ + DO_NOTHING = 0, + STOP = 1, + SLOWDOWN = 2, + APPROACH = 3, + LIMIT = 4, +}; + +class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor +{ +public: + void start() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_activate(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void stop() + { + ASSERT_EQ(on_deactivate(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_cleanup(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void configure() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void cant_configure() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::FAILURE); + } + + bool correctDataReceived(const double expected_dist, const rclcpp::Time & stamp) + { + for (std::shared_ptr source : sources_) { + std::vector collision_points; + source->getData(stamp, collision_points); + if (collision_points.size() != 0) { + const double dist = std::hypot(collision_points[0].x, collision_points[0].y); + if (std::fabs(dist - expected_dist) <= EPSILON) { + return true; + } + } + } + return false; + } +}; // CollisionMonitorWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + + // Configuring + void setCommonParameters(); + void addPolygon( + const std::string & polygon_name, const PolygonType type, + const double size, const std::string & at, + const std::vector & sources_names = std::vector()); + 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); + + // Publish robot footprint + void publishFootprint(const double radius, const rclcpp::Time & stamp); + + // Main topic/data working routines + void publishScan(const double dist, const rclcpp::Time & stamp); + void publishPointCloud(const double dist, const rclcpp::Time & stamp); + void publishRange(const double dist, const rclcpp::Time & stamp); + void publishPolygon(const double dist, const rclcpp::Time & stamp); + void publishCmdVel(const double x, const double y, const double tw); + bool waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp); + bool waitCmdVel(const std::chrono::nanoseconds & timeout); + bool waitFuture( + rclcpp::Client::SharedFuture, + const std::chrono::nanoseconds & timeout); + bool waitActionState(const std::chrono::nanoseconds & timeout); + bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout); + +protected: + void cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg); + void actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPtr msg); + void collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg); + + // CollisionMonitor node + std::shared_ptr cm_; + + // Footprint publisher + rclcpp::Publisher::SharedPtr footprint_pub_; + + // Data source publishers + rclcpp::Publisher::SharedPtr scan_pub_; + rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr range_pub_; + rclcpp::Publisher::SharedPtr polygon_source_pub_; + + // Working with cmd_vel_in/cmd_vel_out + rclcpp::Publisher::SharedPtr cmd_vel_in_pub_; + rclcpp::Subscription::SharedPtr cmd_vel_out_sub_; + + geometry_msgs::msg::Twist::SharedPtr cmd_vel_out_; + + // CollisionMonitor Action state + rclcpp::Subscription::SharedPtr action_state_sub_; + nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; + + // CollisionMonitor collision points markers + rclcpp::Subscription::SharedPtr + collision_points_marker_sub_; + visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; + + // Service client for setting CollisionMonitor parameters + rclcpp::Client::SharedPtr parameters_client_; +}; // Tester + +Tester::Tester() +{ + cm_ = std::make_shared(); + + footprint_pub_ = cm_->create_publisher( + FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + scan_pub_ = cm_->create_publisher( + SCAN_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + pointcloud_pub_ = cm_->create_publisher( + POINTCLOUD_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + range_pub_ = cm_->create_publisher( + RANGE_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + polygon_source_pub_ = cm_->create_publisher( + POLYGON_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + cmd_vel_in_pub_ = cm_->create_publisher( + CMD_VEL_IN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + cmd_vel_out_sub_ = cm_->create_subscription( + CMD_VEL_OUT_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::cmdVelOutCallback, this, std::placeholders::_1)); + + action_state_sub_ = cm_->create_subscription( + STATE_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::actionStateCallback, this, std::placeholders::_1)); + collision_points_marker_sub_ = cm_->create_subscription( + COLLISION_POINTS_MARKERS_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::collisionPointsMarkerCallback, this, std::placeholders::_1)); + parameters_client_ = + cm_->create_client( + std::string( + cm_->get_name()) + "/set_parameters"); +} + +Tester::~Tester() +{ + footprint_pub_.reset(); + + scan_pub_.reset(); + pointcloud_pub_.reset(); + range_pub_.reset(); + polygon_source_pub_.reset(); + + cmd_vel_in_pub_.reset(); + cmd_vel_out_sub_.reset(); + + action_state_sub_.reset(); + collision_points_marker_sub_.reset(); + + cm_.reset(); +} + +void Tester::setCommonParameters() +{ + cm_->declare_parameter( + "cmd_vel_in_topic", rclcpp::ParameterValue(CMD_VEL_IN_TOPIC)); + cm_->set_parameter( + rclcpp::Parameter("cmd_vel_in_topic", CMD_VEL_IN_TOPIC)); + cm_->declare_parameter( + "cmd_vel_out_topic", rclcpp::ParameterValue(CMD_VEL_OUT_TOPIC)); + cm_->set_parameter( + rclcpp::Parameter("cmd_vel_out_topic", CMD_VEL_OUT_TOPIC)); + cm_->declare_parameter( + "state_topic", rclcpp::ParameterValue(STATE_TOPIC)); + cm_->set_parameter( + rclcpp::Parameter("state_topic", STATE_TOPIC)); + + cm_->declare_parameter( + "base_frame_id", rclcpp::ParameterValue(BASE_FRAME_ID)); + cm_->set_parameter( + rclcpp::Parameter("base_frame_id", BASE_FRAME_ID)); + cm_->declare_parameter( + "odom_frame_id", rclcpp::ParameterValue(ODOM_FRAME_ID)); + cm_->set_parameter( + rclcpp::Parameter("odom_frame_id", ODOM_FRAME_ID)); + + cm_->declare_parameter( + "transform_tolerance", rclcpp::ParameterValue(TRANSFORM_TOLERANCE)); + cm_->set_parameter( + rclcpp::Parameter("transform_tolerance", TRANSFORM_TOLERANCE)); + cm_->declare_parameter( + "source_timeout", rclcpp::ParameterValue(SOURCE_TIMEOUT)); + cm_->set_parameter( + rclcpp::Parameter("source_timeout", SOURCE_TIMEOUT)); + + cm_->declare_parameter( + "stop_pub_timeout", rclcpp::ParameterValue(STOP_PUB_TIMEOUT)); + cm_->set_parameter( + rclcpp::Parameter("stop_pub_timeout", STOP_PUB_TIMEOUT)); +} + +void Tester::addPolygon( + const std::string & polygon_name, const PolygonType type, + const double size, const std::string & at, + const std::vector & sources_names) +{ + if (type == POLYGON) { + cm_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + if (at != "approach") { + 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 + ".points", rclcpp::ParameterValue(points)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", points)); + } else { // at == "approach" + cm_->declare_parameter( + polygon_name + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".footprint_topic", FOOTPRINT_TOPIC)); + } + } else if (type == CIRCLE) { + cm_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("circle")); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "circle")); + + cm_->declare_parameter( + 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")); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "unknown")); + } + + cm_->declare_parameter( + polygon_name + ".enabled", rclcpp::ParameterValue(true)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".enabled", true)); + + cm_->declare_parameter( + polygon_name + ".action_type", rclcpp::ParameterValue(at)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".action_type", at)); + + cm_->declare_parameter( + polygon_name + ".min_points", rclcpp::ParameterValue(MIN_POINTS)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".min_points", MIN_POINTS)); + + cm_->declare_parameter( + polygon_name + ".slowdown_ratio", rclcpp::ParameterValue(SLOWDOWN_RATIO)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO)); + + cm_->declare_parameter( + polygon_name + ".linear_limit", rclcpp::ParameterValue(LINEAR_LIMIT)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".linear_limit", LINEAR_LIMIT)); + + cm_->declare_parameter( + polygon_name + ".angular_limit", rclcpp::ParameterValue(ANGULAR_LIMIT)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".angular_limit", ANGULAR_LIMIT)); + + cm_->declare_parameter( + polygon_name + ".time_before_collision", rclcpp::ParameterValue(TIME_BEFORE_COLLISION)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".time_before_collision", TIME_BEFORE_COLLISION)); + + cm_->declare_parameter( + polygon_name + ".simulation_time_step", rclcpp::ParameterValue(SIMULATION_TIME_STEP)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".simulation_time_step", SIMULATION_TIME_STEP)); + + cm_->declare_parameter( + polygon_name + ".visualize", rclcpp::ParameterValue(false)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".visualize", false)); + + cm_->declare_parameter( + polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".polygon_pub_topic", polygon_name)); + + if (!sources_names.empty()) { + cm_->declare_parameter( + polygon_name + ".sources_names", rclcpp::ParameterValue(sources_names)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".sources_names", sources_names)); + } +} + +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) +{ + if (type == SCAN) { + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("scan")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "scan")); + } else if (type == POINTCLOUD) { + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("pointcloud")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "pointcloud")); + + cm_->declare_parameter( + source_name + ".min_height", rclcpp::ParameterValue(0.1)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".min_height", 0.1)); + cm_->declare_parameter( + source_name + ".max_height", rclcpp::ParameterValue(1.0)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".max_height", 1.0)); + } else if (type == RANGE) { + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("range")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "range")); + + cm_->declare_parameter( + source_name + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 200)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".obstacles_angle", M_PI / 200)); + } else if (type == POLYGON_SOURCE) { + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("polygon")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "polygon")); + + cm_->declare_parameter( + source_name + ".sampling_distance", rclcpp::ParameterValue(0.1)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".sampling_distance", 0.1)); + cm_->declare_parameter( + source_name + ".polygon_similarity_threshold", rclcpp::ParameterValue(2.0)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".polygon_similarity_threshold", 2.0)); + } else { // type == SOURCE_UNKNOWN + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("unknown")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "unknown")); + } + + cm_->declare_parameter( + source_name + ".topic", rclcpp::ParameterValue(source_name)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".topic", source_name)); +} + +void Tester::setVectors( + const std::vector & polygons, + const std::vector & sources) +{ + cm_->declare_parameter("polygons", rclcpp::ParameterValue(polygons)); + cm_->set_parameter(rclcpp::Parameter("polygons", polygons)); + + cm_->declare_parameter("observation_sources", rclcpp::ParameterValue(sources)); + 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 = + std::make_shared(cm_); + + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + // Fill TF buffer ahead for future transform usage in CollisionMonitor::process() + const rclcpp::Duration ahead = 1000ms; + for (rclcpp::Time t = stamp; t <= stamp + ahead; t += rclcpp::Duration(50ms)) { + transform.header.stamp = t; + + // base_frame -> source_frame transform + transform.header.frame_id = BASE_FRAME_ID; + transform.child_frame_id = SOURCE_FRAME_ID; + tf_broadcaster->sendTransform(transform); + + // odom_frame -> base_frame transform + transform.header.frame_id = ODOM_FRAME_ID; + transform.child_frame_id = BASE_FRAME_ID; + tf_broadcaster->sendTransform(transform); + } +} + +void Tester::publishFootprint(const double radius, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = BASE_FRAME_ID; + msg->header.stamp = stamp; + + geometry_msgs::msg::Point32 p; + p.x = radius; + p.y = radius; + msg->polygon.points.push_back(p); + p.x = radius; + p.y = -radius; + msg->polygon.points.push_back(p); + p.x = -radius; + p.y = -radius; + msg->polygon.points.push_back(p); + p.x = -radius; + p.y = radius; + msg->polygon.points.push_back(p); + + footprint_pub_->publish(std::move(msg)); +} + +void Tester::publishScan(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->angle_min = 0.0; + msg->angle_max = 2 * M_PI; + msg->angle_increment = M_PI / 180; + msg->time_increment = 0.0; + msg->scan_time = 0.0; + msg->range_min = 0.1; + msg->range_max = dist + 1.0; + std::vector ranges(360, dist); + msg->ranges = ranges; + + scan_pub_->publish(std::move(msg)); +} + +void Tester::publishPointCloud(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*msg); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + modifier.resize(2); + + sensor_msgs::PointCloud2Iterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); + + // Point 0: (dist, 0.01, 0.2) + *iter_x = dist; + *iter_y = 0.01; + *iter_z = 0.2; + ++iter_x; ++iter_y; ++iter_z; + + // Point 1: (dist, -0.01, 0.2) + *iter_x = dist; + *iter_y = -0.01; + *iter_z = 0.2; + + pointcloud_pub_->publish(std::move(msg)); +} + +void Tester::publishRange(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->radiation_type = 0; + msg->field_of_view = M_PI / 10; + msg->min_range = 0.1; + msg->max_range = dist + 0.1; + msg->range = dist; + + range_pub_->publish(std::move(msg)); +} + +void Tester::publishPolygon(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + p.x = 1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + + polygon_source_pub_->publish(std::move(msg)); +} + +void Tester::publishCmdVel(const double x, const double y, const double tw) +{ + // Reset cmd_vel_out_ before calling CollisionMonitor::process() + cmd_vel_out_ = nullptr; + action_state_ = nullptr; + collision_points_marker_msg_ = nullptr; + + std::unique_ptr msg = + std::make_unique(); + + msg->linear.x = x; + msg->linear.y = y; + msg->angular.z = tw; + + cmd_vel_in_pub_->publish(std::move(msg)); +} + +bool Tester::waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + if (cm_->correctDataReceived(expected_dist, stamp)) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitCmdVel(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + if (cmd_vel_out_) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitFuture( + rclcpp::Client::SharedFuture result_future, + const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + std::future_status status = result_future.wait_for(10ms); + if (status == std::future_status::ready) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + if (action_state_) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + if (collision_points_marker_msg_) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +void Tester::cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg) +{ + cmd_vel_out_ = msg; +} + +void Tester::actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPtr msg) +{ + action_state_ = msg; +} + +void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg) +{ + collision_points_marker_msg_ = msg; +} + +TEST_F(Tester, testProcessStopSlowdownLimit) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner for robot stop. + setCommonParameters(); + addPolygon("Limit", POLYGON, 3.0, "limit"); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + setVectors({"Limit", "SlowDown", "Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is far away from robot + publishScan(4.5, curr_time); + ASSERT_TRUE(waitData(4.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + + // 2. Obstacle is in limit robot zone + publishScan(3.0, curr_time); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2); + const double ratio = LINEAR_LIMIT / speed; + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, LIMIT); + ASSERT_EQ(action_state_->polygon_name, "Limit"); + + // 3. Obstacle is in slowdown robot zone + publishScan(1.5, curr_time); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * SLOWDOWN_RATIO, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2 * SLOWDOWN_RATIO, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1 * SLOWDOWN_RATIO, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, SLOWDOWN); + ASSERT_EQ(action_state_->polygon_name, "SlowDown"); + + // 4. Obstacle is inside stop zone + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 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, "Stop"); + + // 5. Restoring back normal operation + publishScan(4.5, curr_time); + ASSERT_TRUE(waitData(4.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, 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, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testPolygonSource) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner for robot stop. + setCommonParameters(); + // Set source_timeout to 0.0 to clear out quickly the polygons from test to test + cm_->set_parameter( + rclcpp::Parameter("source_timeout", 0.1)); + addPolygon("Limit", POLYGON, 3.0, "limit"); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(POLYGON_NAME, POLYGON_SOURCE); + setVectors({"Limit", "SlowDown", "Stop"}, {POLYGON_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is far away from robot + publishPolygon(4.5, curr_time); + ASSERT_TRUE(waitData(std::hypot(1.0, 4.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + + // 2. Obstacle is in limit robot zone + publishPolygon(3.0, curr_time); + EXPECT_TRUE(waitData(std::hypot(1.0, 3.0), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + EXPECT_TRUE(waitCmdVel(500ms)); + const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2); + const double ratio = LINEAR_LIMIT / speed; + EXPECT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON); + EXPECT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON); + EXPECT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON); + EXPECT_TRUE(waitActionState(500ms)); + EXPECT_EQ(action_state_->action_type, LIMIT); + EXPECT_EQ(action_state_->polygon_name, "Limit"); + + // 3. Obstacle is in slowdown robot zone + publishPolygon(1.5, curr_time); + EXPECT_TRUE(waitData(std::hypot(1.0, 1.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + EXPECT_TRUE(waitCmdVel(500ms)); + EXPECT_NEAR(cmd_vel_out_->linear.x, 0.5 * SLOWDOWN_RATIO, EPSILON); + EXPECT_NEAR(cmd_vel_out_->linear.y, 0.2 * SLOWDOWN_RATIO, EPSILON); + EXPECT_NEAR(cmd_vel_out_->angular.z, 0.1 * SLOWDOWN_RATIO, EPSILON); + EXPECT_TRUE(waitActionState(500ms)); + EXPECT_EQ(action_state_->action_type, SLOWDOWN); + EXPECT_EQ(action_state_->polygon_name, "SlowDown"); + + // 4. Obstacle is inside stop zone + curr_time = cm_->now(); + publishPolygon(0.5, curr_time); + EXPECT_TRUE(waitData(std::hypot(1.0, 0.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + EXPECT_TRUE(waitCmdVel(500ms)); + EXPECT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + EXPECT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + EXPECT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + EXPECT_TRUE(waitActionState(500ms)); + EXPECT_EQ(action_state_->action_type, STOP); + EXPECT_EQ(action_state_->polygon_name, "Stop"); + + // 5. Restoring back normal operation + publishPolygon(4.5, curr_time); + ASSERT_TRUE(waitData(std::hypot(1.0, 4.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, 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, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testProcessApproach) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making one circle footprint for approach. + setCommonParameters(); + addPolygon("Approach", CIRCLE, 1.0, "approach"); + addSource(SCAN_NAME, SCAN); + setVectors({"Approach"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is far away from robot + publishScan(3.0, curr_time); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // 2. Approaching obstacle (0.2 m ahead from robot footprint) + publishScan(1.2, curr_time); + ASSERT_TRUE(waitData(1.2, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + // change_ratio = (0.2 m / speed m/s) / TIME_BEFORE_COLLISION s + // where speed = sqrt(0.5^2 + 0.2^2) ~= 0.538516481 m/s + const double change_ratio = (0.2 / 0.538516481) / TIME_BEFORE_COLLISION; + ASSERT_NEAR( + cmd_vel_out_->linear.x, 0.5 * change_ratio, 0.5 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION); + ASSERT_NEAR( + cmd_vel_out_->linear.y, 0.2 * change_ratio, 0.2 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, APPROACH); + ASSERT_EQ(action_state_->polygon_name, "Approach"); + + // 3. Obstacle is inside robot footprint + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + // Publish impossible cmd_vel to ensure robot footprint is checked + publishCmdVel(1000000000.0, 0.2, 0.0); + 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); + + // 4. Restoring back normal operation + publishScan(3.0, curr_time); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testProcessApproachRotation) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making one circle footprint for approach. + setCommonParameters(); + addPolygon("Approach", POLYGON, 1.0, "approach"); + addSource(RANGE_NAME, RANGE); + setVectors({"Approach"}, {RANGE_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Publish robot footprint + publishFootprint(1.0, curr_time); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is far away from robot + publishRange(2.0, curr_time); + ASSERT_TRUE(waitData(2.0, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + 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, M_PI / 4, EPSILON); + + // 2. Approaching rotation to obstacle ( M_PI / 4 - M_PI / 20 ahead from robot) + publishRange(1.4, curr_time); + ASSERT_TRUE(waitData(1.4, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + 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, + M_PI / 5, + (M_PI / 4) * (SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION)); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, APPROACH); + ASSERT_EQ(action_state_->polygon_name, "Approach"); + + // 3. Obstacle is inside robot footprint + publishRange(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + 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); + + // 4. Restoring back normal operation + publishRange(2.5, curr_time); + ASSERT_TRUE(waitData(2.5, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + 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, M_PI / 4, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testCrossOver) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner circle + // as robot footprint for approach. + setCommonParameters(); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Approach", CIRCLE, 1.0, "approach"); + addSource(POINTCLOUD_NAME, POINTCLOUD); + addSource(RANGE_NAME, RANGE); + setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m). + // Robot should approach the obstacle. + publishPointCloud(2.5, curr_time); + publishRange(2.5, curr_time); + ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); + publishCmdVel(3.0, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + // change_ratio = (1.5 m / 3.0 m/s) / TIME_BEFORE_COLLISION s + double change_ratio = (1.5 / 3.0) / TIME_BEFORE_COLLISION; + ASSERT_NEAR( + cmd_vel_out_->linear.x, 3.0 * change_ratio, 3.0 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION); + 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, APPROACH); + ASSERT_EQ(action_state_->polygon_name, "Approach"); + + // 2. Obstacle is inside slowdown zone, but speed is too slow for approach + publishRange(1.5, curr_time); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); + publishCmdVel(0.1, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.1 * SLOWDOWN_RATIO, 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, SLOWDOWN); + ASSERT_EQ(action_state_->polygon_name, "SlowDown"); + + // 3. Increase robot speed to return again into approach mode. + // The speed should be safer for approach mode, so robot will go to the approach (ahead in 0.5 m) + // even while it is already inside slowdown area. + publishCmdVel(1.0, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + // change_ratio = (0.5 m / 1.0 m/s) / TIME_BEFORE_COLLISION s + change_ratio = (0.5 / 1.0) / TIME_BEFORE_COLLISION; + ASSERT_NEAR( + cmd_vel_out_->linear.x, 1.0 * change_ratio, 1.0 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION); + 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, APPROACH); + ASSERT_EQ(action_state_->polygon_name, "Approach"); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testSourceTimeout) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner circle + // as robot footprint for approach. + setCommonParameters(); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Approach", CIRCLE, 1.0, "approach"); + addSource(POINTCLOUD_NAME, POINTCLOUD); + addSource(RANGE_NAME, RANGE); + setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m). + // Robot should approach the obstacle. + publishPointCloud(2.5, curr_time); + ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); + publishCmdVel(3.0, 3.0, 3.0); + ASSERT_TRUE(waitCmdVel(500ms)); + // Range configured but not published, range source should be considered invalid + ASSERT_NEAR(cmd_vel_out_->linear.y, 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, "invalid source"); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testSourceTimeoutOverride) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner circle + // as robot footprint for approach. + setCommonParameters(); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Approach", CIRCLE, 1.0, "approach"); + addSource(POINTCLOUD_NAME, POINTCLOUD); + addSource(RANGE_NAME, RANGE); + setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); + cm_->set_parameter(rclcpp::Parameter("source_timeout", 0.0)); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m). + // Robot should approach the obstacle. + publishPointCloud(2.5, curr_time); + ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); + publishCmdVel(3.0, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + // change_ratio = (1.5 m / 3.0 m/s) / TIME_BEFORE_COLLISION s + double change_ratio = (1.5 / 3.0) / TIME_BEFORE_COLLISION; + // Range configured but not published, range source should be considered invalid + // but as we set the source_timeout of the Range source to 0.0, its validity check is overidden + ASSERT_NEAR( + cmd_vel_out_->linear.x, 3.0 * change_ratio, 3.0 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION); + 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, APPROACH); + ASSERT_EQ(action_state_->polygon_name, "Approach"); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testCeasePublishZeroVel) +{ + rclcpp::Time curr_time = cm_->now(); + + // Configure stop and approach zones, and basic data source + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addPolygon("Approach", CIRCLE, 2.0, "approach"); + addSource(SCAN_NAME, SCAN); + setVectors({"Stop", "Approach"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is inside approach footprint: robot should stop + publishScan(1.5, curr_time); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 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, APPROACH); + ASSERT_EQ(action_state_->polygon_name, "Approach"); + + // Wait more than STOP_PUB_TIMEOUT time + std::this_thread::sleep_for(std::chrono::duration(STOP_PUB_TIMEOUT + 0.01)); + + // 2. Check that zero cmd_vel_out velocity won't be published more for this case + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_FALSE(waitCmdVel(100ms)); + + // 3. Release robot to normal operation + publishScan(3.0, curr_time); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, 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 inside stop zone + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 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, "Stop"); + + // Wait more than STOP_PUB_TIMEOUT time + std::this_thread::sleep_for(std::chrono::duration(STOP_PUB_TIMEOUT + 0.01)); + + // 5. Check that zero cmd_vel_out velocity won't be published more for this case + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_FALSE(waitCmdVel(100ms)); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testPolygonNotEnabled) +{ + // Set Collision Monitor parameters. + // Create a STOP polygon + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + // Create a Scan source + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Check that robot stops when polygon is enabled + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 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, "Stop"); + + // Disable polygon by calling service + auto set_parameters_msg = std::make_shared(); + auto parameter_msg = std::make_shared(); + parameter_msg->name = "Stop.enabled"; + parameter_msg->value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + parameter_msg->value.bool_value = false; + set_parameters_msg->parameters.push_back(*parameter_msg); + auto result_future = parameters_client_->async_send_request(set_parameters_msg).future.share(); + ASSERT_TRUE(waitFuture(result_future, 2s)); + + // Check that robot does not stop when polygon is disabled + curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, 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, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testSourceNotEnabled) +{ + // Set Collision Monitor parameters. + // Create a STOP polygon + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + // Create a Scan source + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Check that robot stops when source is enabled + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 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, "Stop"); + + // Disable source by calling service + auto set_parameters_msg = std::make_shared(); + auto parameter_msg = std::make_shared(); + parameter_msg->name = std::string(SCAN_NAME) + ".enabled"; + parameter_msg->value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + parameter_msg->value.bool_value = false; + set_parameters_msg->parameters.push_back(*parameter_msg); + auto result_future = parameters_client_->async_send_request(set_parameters_msg).future.share(); + ASSERT_TRUE(waitFuture(result_future, 2s)); + + // Check that robot does not stop when source is disabled + curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, 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, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testProcessNonActive) +{ + rclcpp::Time curr_time = cm_->now(); + + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Configure Collision Monitor node, but not activate + cm_->configure(); + + // Share TF + sendTransforms(curr_time); + + // Call CollisionMonitor::process() + publishCmdVel(1.0, 0.0, 0.0); + // ... and check that cmd_vel_out was not published + ASSERT_FALSE(waitCmdVel(100ms)); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testIncorrectPolygonType) +{ + setCommonParameters(); + addPolygon("UnknownShape", POLYGON_UNKNOWN, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + setVectors({"UnknownShape"}, {SCAN_NAME}); + + // Check that Collision Monitor node can not be configured for this parameters set + cm_->cant_configure(); +} + +TEST_F(Tester, testIncorrectSourceType) +{ + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource("UnknownSource", SOURCE_UNKNOWN); + setVectors({"Stop"}, {"UnknownSource"}); + + // Check that Collision Monitor node can not be configured for this parameters set + cm_->cant_configure(); +} + +TEST_F(Tester, testPolygonsNotSet) +{ + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + + // Check that Collision Monitor node can not be configured for this parameters set + cm_->cant_configure(); +} + +TEST_F(Tester, testSourcesNotSet) +{ + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + cm_->declare_parameter("polygons", rclcpp::ParameterValue(std::vector{"Stop"})); + cm_->set_parameter(rclcpp::Parameter("polygons", std::vector{"Stop"})); + + // Check that Collision Monitor node can not be configured for this parameters set + 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); + + // No source published, empty marker array published + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_EQ(collision_points_marker_msg_->markers.size(), 0u); + + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + 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(); +} + +TEST_F(Tester, testSourceAssociatedToPolygon) +{ + // Set Collision Monitor parameters: + // - 2 sources (scan and range) + // - 1 stop polygon associated to range source + // - 1 slowdown polygon (associated with all sources by default) + setCommonParameters(); + addSource(SCAN_NAME, SCAN); + addSource(RANGE_NAME, RANGE); + std::vector range_only_sources_names = {RANGE_NAME}; + std::vector all_sources_names = {SCAN_NAME, RANGE_NAME}; + addPolygon("StopOnRangeSource", POLYGON, 1.0, "stop", range_only_sources_names); + addPolygon("SlowdownOnAllSources", POLYGON, 1.0, "slowdown"); + setVectors({"StopOnRangeSource", "SlowdownOnAllSources"}, {SCAN_NAME, RANGE_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + + // Publish sources so that : + // - scan obstacle is in polygons + // - range obstacle is far away from polygons + publishScan(0.5, curr_time); + publishRange(4.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + + // Publish cmd vel + publishCmdVel(0.5, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + + // Since the stop polygon is only checking range source, slowdown action should be applied + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, SLOWDOWN); + ASSERT_EQ(action_state_->polygon_name, "SlowdownOnAllSources"); + + // Stop Collision Monitor node + cm_->stop(); +} + +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; +} diff --git a/nav2_collision_monitor/test/kinematics_test.cpp b/nav2_collision_monitor/test/kinematics_test.cpp new file mode 100644 index 00000000000..65933daa389 --- /dev/null +++ b/nav2_collision_monitor/test/kinematics_test.cpp @@ -0,0 +1,98 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// +// 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 "rclcpp/rclcpp.hpp" + +#include "nav2_collision_monitor/types.hpp" +#include "nav2_collision_monitor/kinematics.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(KinematicsTest, testTransformPoints) +{ + // Transform: move frame to (2.0, 1.0) coordinate and rotate it on 30 degrees + const nav2_collision_monitor::Pose tf{2.0, 1.0, M_PI / 6.0}; + // Add two points in the basic frame + std::vector points; + points.push_back({3.0, 2.0}); + points.push_back({0.0, 0.0}); + + // Transform points from basic frame to the new frame + nav2_collision_monitor::transformPoints(tf, points); + + // Check that all points were transformed correctly + // Distance to point in a new frame + double new_point_distance = std::sqrt(1.0 + 1.0); + // Angle of point in a new frame. Calculated as: + // angle of point in a moved frame - frame rotation. + double new_point_angle = M_PI / 4.0 - M_PI / 6.0; + EXPECT_NEAR(points[0].x, new_point_distance * std::cos(new_point_angle), EPSILON); + EXPECT_NEAR(points[0].y, new_point_distance * std::sin(new_point_angle), EPSILON); + + new_point_distance = std::sqrt(1.0 + 4.0); + new_point_angle = M_PI + std::atan(1.0 / 2.0) - M_PI / 6.0; + EXPECT_NEAR(points[1].x, new_point_distance * std::cos(new_point_angle), EPSILON); + EXPECT_NEAR(points[1].y, new_point_distance * std::sin(new_point_angle), EPSILON); +} + +TEST(KinematicsTest, testProjectState) +{ + // Y Y + // ^ ^ + // ' ' + // ' ==> ' * + // ' * <- robot's nose 2.0' o <- moved robot + // 1.0' o <- robot's back ' + // ..........>X ..........>X + // 2.0 2.0 + + // Initial pose of robot + nav2_collision_monitor::Pose pose{2.0, 1.0, M_PI / 4.0}; + // Initial velocity of robot + nav2_collision_monitor::Velocity vel{0.0, 1.0, M_PI / 4.0}; + const double dt = 1.0; + + // Moving robot and rotating velocity + nav2_collision_monitor::projectState(dt, pose, vel); + + // Check pose of moved and rotated robot + EXPECT_NEAR(pose.x, 2.0, EPSILON); + EXPECT_NEAR(pose.y, 2.0, EPSILON); + EXPECT_NEAR(pose.theta, M_PI / 2, EPSILON); + + // Check rotated velocity + // Rotated velocity angle is an initial velocity angle + rotation + const double rotated_vel_angle = M_PI / 2.0 + M_PI / 4.0; + EXPECT_NEAR(vel.x, std::cos(rotated_vel_angle), EPSILON); + EXPECT_NEAR(vel.y, std::sin(rotated_vel_angle), EPSILON); + EXPECT_NEAR(vel.tw, M_PI / 4.0, EPSILON); // should be the same +} diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp new file mode 100644 index 00000000000..a0dbf9fcc96 --- /dev/null +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -0,0 +1,1055 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// +// 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/circle.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 FOOTPRINT_TOPIC[]{"footprint"}; +static const char POLYGON_SUB_TOPIC[]{"polygon_sub"}; +static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; +static const char POLYGON_NAME[]{"TestPolygon"}; +static const char CIRCLE_NAME[]{"TestCircle"}; +static const char OBSERVATION_SOURCE_NAME[]{"source"}; +static const std::vector SQUARE_POLYGON { + 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5}; +static const std::vector ARBITRARY_POLYGON { + 1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 2.0, -1.0, -1.0, -1.0, -1.0, 1.0}; +static const char SQUARE_POLYGON_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"}; +static const char ARBITRARY_POLYGON_STR[]{ + "[[1.0, 1.0], [1.0, 0.0], [2.0, 0.0], [2.0, -1.0], [-1.0, -1.0], [-1.0, 1.0]]"}; +static const char INCORRECT_POINTS_1_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5]]" +}; +static const char INCORRECT_POINTS_2_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5], [0]]" +}; +static const char INVALID_POINTS_STR[]{ + "[[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5], 0]]" +}; +static const double CIRCLE_RADIUS{0.5}; +static const int MIN_POINTS{2}; +static const double SLOWDOWN_RATIO{0.7}; +static const double LINEAR_LIMIT{0.4}; +static const double ANGULAR_LIMIT{0.09}; +static const double TIME_BEFORE_COLLISION{1.0}; +static const double SIMULATION_TIME_STEP{0.01}; +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() + { + polygon_pub_.reset(); + footprint_pub_.reset(); + } + + void publishPolygon(const std::string & frame_id, const bool is_correct) + { + polygon_pub_ = this->create_publisher( + POLYGON_SUB_TOPIC, rclcpp::SystemDefaultsQoS()); + + std::unique_ptr msg = + std::make_unique(); + + unsigned int polygon_size; + if (is_correct) { + polygon_size = SQUARE_POLYGON.size(); + } else { + polygon_size = 2; + } + + msg->header.frame_id = frame_id; + msg->header.stamp = this->now(); + + geometry_msgs::msg::Point32 p; + for (unsigned int i = 0; i < polygon_size; i = i + 2) { + p.x = SQUARE_POLYGON[i]; + p.y = SQUARE_POLYGON[i + 1]; + msg->polygon.points.push_back(p); + } + + polygon_pub_->publish(std::move(msg)); + } + + void publishRadius() + { + radius_pub_ = this->create_publisher( + POLYGON_SUB_TOPIC, rclcpp::SystemDefaultsQoS()); + + std::unique_ptr msg = std::make_unique(); + msg->data = CIRCLE_RADIUS; + + radius_pub_->publish(std::move(msg)); + } + + void publishFootprint() + { + footprint_pub_ = this->create_publisher( + FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = BASE_FRAME_ID; + msg->header.stamp = this->now(); + + geometry_msgs::msg::Point32 p; + for (unsigned int i = 0; i < SQUARE_POLYGON.size(); i = i + 2) { + p.x = SQUARE_POLYGON[i]; + p.y = SQUARE_POLYGON[i + 1]; + msg->polygon.points.push_back(p); + } + + footprint_pub_->publish(std::move(msg)); + } + + 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::Publisher::SharedPtr polygon_pub_; + rclcpp::Publisher::SharedPtr radius_pub_; + rclcpp::Publisher::SharedPtr footprint_pub_; + rclcpp::Subscription::SharedPtr polygon_sub_; + + geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received_; +}; // TestNode + +class PolygonWrapper : public nav2_collision_monitor::Polygon +{ +public: + PolygonWrapper( + 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::Polygon( + node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) + { + } + + double getSimulationTimeStep() const + { + return simulation_time_step_; + } + + double isVisualize() const + { + return visualize_; + } +}; // PolygonWrapper + +class CircleWrapper : public nav2_collision_monitor::Circle +{ +public: + CircleWrapper( + 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::Circle( + node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) + { + } + + double getRadius() const + { + return radius_; + } + + double getRadiusSquared() const + { + return radius_squared_; + } +}; // CircleWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + +protected: + // Working with parameters + void setCommonParameters( + const std::string & polygon_name, const std::string & action_type, + const std::vector & observation_sources = + std::vector({OBSERVATION_SOURCE_NAME}), + const std::vector & sources_names = std::vector()); + void setPolygonParameters( + const char * points, + const bool is_static); + void setCircleParameters(const double radius, const bool is_static); + bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param); + // Creating routines + void createPolygon(const std::string & action_type, const bool is_static); + void createCircle(const std::string & action_type, const bool is_static); + + void sendTransforms(double shift); + + // Wait until polygon will be received + bool waitPolygon( + const std::chrono::nanoseconds & timeout, + std::vector & poly); + + // Wait until circle polygon radius will be received + bool waitRadius(const std::chrono::nanoseconds & timeout); + + // Wait until footprint will be received + bool waitFootprint( + const std::chrono::nanoseconds & timeout, + std::vector & footprint); + + std::shared_ptr test_node_; + + std::shared_ptr polygon_; + std::shared_ptr circle_; + + 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() +{ + polygon_.reset(); + circle_.reset(); + + test_node_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::setCommonParameters( + const std::string & polygon_name, const std::string & action_type, + const std::vector & observation_sources, + const std::vector & sources_names) +{ + 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 + ".slowdown_ratio", rclcpp::ParameterValue(SLOWDOWN_RATIO)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO)); + + test_node_->declare_parameter( + polygon_name + ".linear_limit", rclcpp::ParameterValue(LINEAR_LIMIT)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".linear_limit", LINEAR_LIMIT)); + + test_node_->declare_parameter( + polygon_name + ".angular_limit", rclcpp::ParameterValue(ANGULAR_LIMIT)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".angular_limit", ANGULAR_LIMIT)); + + test_node_->declare_parameter( + polygon_name + ".time_before_collision", + rclcpp::ParameterValue(TIME_BEFORE_COLLISION)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".time_before_collision", TIME_BEFORE_COLLISION)); + + test_node_->declare_parameter( + polygon_name + ".simulation_time_step", rclcpp::ParameterValue(SIMULATION_TIME_STEP)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".simulation_time_step", SIMULATION_TIME_STEP)); + + 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)); + + test_node_->declare_parameter( + "observation_sources", rclcpp::ParameterValue(observation_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", observation_sources)); + + if (!sources_names.empty()) { + test_node_->declare_parameter( + polygon_name + ".sources_names", rclcpp::ParameterValue(sources_names)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".sources_names", sources_names)); + } +} + +void Tester::setPolygonParameters( + const char * points, const bool is_static) +{ + if (is_static) { + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(points)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", points)); + } else { + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".polygon_sub_topic", rclcpp::ParameterValue(POLYGON_SUB_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".polygon_sub_topic", POLYGON_SUB_TOPIC)); + + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".footprint_topic", FOOTPRINT_TOPIC)); + } +} + +void Tester::setCircleParameters(const double radius, const bool is_static) +{ + if (is_static) { + test_node_->declare_parameter( + std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius)); + } else { + test_node_->declare_parameter( + std::string(CIRCLE_NAME) + ".polygon_sub_topic", rclcpp::ParameterValue(POLYGON_SUB_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".polygon_sub_topic", POLYGON_SUB_TOPIC)); + } +} + +bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const std::string & param) +{ + bool ret = false; + + // Check that parameter is not set after configuring + try { + test_node_->get_parameter(polygon_name + "." + param); + } catch (std::exception & ex) { + std::string message = ex.what(); + if (message.find("." + param) != std::string::npos && + message.find("is not initialized") != std::string::npos) + { + ret = true; + } + } + return ret; +} + +void Tester::createPolygon(const std::string & action_type, const bool is_static) +{ + setCommonParameters(POLYGON_NAME, action_type); + setPolygonParameters(SQUARE_POLYGON_STR, is_static); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + polygon_->activate(); +} + +void Tester::createCircle(const std::string & action_type, const bool is_static) +{ + setCommonParameters(CIRCLE_NAME, action_type); + setCircleParameters(CIRCLE_RADIUS, is_static); + + circle_ = std::make_shared( + test_node_, CIRCLE_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(circle_->configure()); + circle_->activate(); +} + +void Tester::sendTransforms(double shift) +{ + std::shared_ptr tf_broadcaster = + std::make_shared(test_node_); + + geometry_msgs::msg::TransformStamped transform; + + // base_frame -> base2_frame transform + transform.header.frame_id = BASE_FRAME_ID; + transform.child_frame_id = BASE2_FRAME_ID; + + transform.header.stamp = test_node_->now(); + transform.transform.translation.x = shift; + transform.transform.translation.y = shift; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); +} + +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)) { + 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; +} + +bool Tester::waitRadius(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (circle_->isShapeSet()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitFootprint( + const std::chrono::nanoseconds & timeout, + std::vector & footprint) +{ + rclcpp::Time start_time = test_node_->now(); + nav2_collision_monitor::Velocity vel{0.0, 0.0, 0.0}; + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + polygon_->updatePolygon(vel); + polygon_->getPolygon(footprint); + if (footprint.size() > 0) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +TEST_F(Tester, testPolygonGetStopParameters) +{ + createPolygon("stop", true); + + // Check that common parameters set correctly + EXPECT_EQ(polygon_->getName(), POLYGON_NAME); + EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::STOP); + EXPECT_EQ(polygon_->getMinPoints(), MIN_POINTS); + EXPECT_EQ(polygon_->isVisualize(), true); + + // Check that polygon set correctly + std::vector poly; + polygon_->getPolygon(poly); + 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, testPolygonGetSlowdownParameters) +{ + createPolygon("slowdown", true); + + // Check that common parameters set correctly + EXPECT_EQ(polygon_->getName(), POLYGON_NAME); + EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::SLOWDOWN); + EXPECT_EQ(polygon_->getMinPoints(), MIN_POINTS); + EXPECT_EQ(polygon_->isVisualize(), true); + // Check that slowdown_ratio is correct + EXPECT_NEAR(polygon_->getSlowdownRatio(), SLOWDOWN_RATIO, EPSILON); +} + +TEST_F(Tester, testPolygonGetLimitParameters) +{ + createPolygon("limit", true); + + // Check that common parameters set correctly + EXPECT_EQ(polygon_->getName(), POLYGON_NAME); + EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::LIMIT); + EXPECT_EQ(polygon_->getMinPoints(), MIN_POINTS); + EXPECT_EQ(polygon_->isVisualize(), true); + // Check that limit params are correct + EXPECT_NEAR(polygon_->getLinearLimit(), LINEAR_LIMIT, EPSILON); + EXPECT_NEAR(polygon_->getAngularLimit(), ANGULAR_LIMIT, EPSILON); +} + +TEST_F(Tester, testPolygonGetApproachParameters) +{ + createPolygon("approach", true); + + // Check that common parameters set correctly + EXPECT_EQ(polygon_->getName(), POLYGON_NAME); + EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::APPROACH); + EXPECT_EQ(polygon_->getMinPoints(), MIN_POINTS); + EXPECT_EQ(polygon_->isVisualize(), true); + // Check that time_before_collision and simulation_time_step are correct + EXPECT_NEAR(polygon_->getTimeBeforeCollision(), TIME_BEFORE_COLLISION, EPSILON); + EXPECT_NEAR(polygon_->getSimulationTimeStep(), SIMULATION_TIME_STEP, EPSILON); +} + +TEST_F(Tester, testCircleGetParameters) +{ + createCircle("approach", true); + + // Check that common parameters set correctly + EXPECT_EQ(circle_->getName(), CIRCLE_NAME); + EXPECT_EQ(circle_->getActionType(), nav2_collision_monitor::APPROACH); + EXPECT_EQ(circle_->getMinPoints(), MIN_POINTS); + + // Check that Circle-specific parameters were set correctly + EXPECT_NEAR(circle_->getRadius(), CIRCLE_RADIUS, EPSILON); + EXPECT_NEAR(circle_->getRadiusSquared(), CIRCLE_RADIUS * CIRCLE_RADIUS, EPSILON); +} + +TEST_F(Tester, testPolygonUndeclaredActionType) +{ + // "action_type" parameter is not initialized + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); + // Check that "action_type" parameter is not set after configuring + ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "action_type")); +} + +TEST_F(Tester, testPolygonUndeclaredPoints) +{ + // "points" and "polygon_sub_topic" parameters are not initialized + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); + // Check that "points" and "polygon_sub_topic" parameters are not set after configuring + ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "points")); + ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "polygon_sub_topic")); +} + +TEST_F(Tester, testPolygonIncorrectActionType) +{ + setCommonParameters(POLYGON_NAME, "incorrect_action_type"); + setPolygonParameters(SQUARE_POLYGON_STR, true); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + +TEST_F(Tester, testPolygonIncorrectPoints1) +{ + setCommonParameters(POLYGON_NAME, "stop"); + + // Triangle points + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INCORRECT_POINTS_1_STR)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INCORRECT_POINTS_1_STR)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + +TEST_F(Tester, testPolygonIncorrectPoints2) +{ + setCommonParameters(POLYGON_NAME, "stop"); + + // Odd number of elements + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INCORRECT_POINTS_2_STR)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INCORRECT_POINTS_2_STR)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + +// Testcase for deprecated "max_points" parameter +TEST_F(Tester, testPolygonMaxPoints) +{ + setCommonParameters(POLYGON_NAME, "stop"); + setPolygonParameters(SQUARE_POLYGON_STR, true); + + const int max_points = 5; + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".max_points", rclcpp::ParameterValue(max_points)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".max_points", max_points)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + EXPECT_EQ(polygon_->getMinPoints(), max_points + 1); +} + +TEST_F(Tester, testCircleUndeclaredRadius) +{ + setCommonParameters(CIRCLE_NAME, "stop"); + + circle_ = std::make_shared( + test_node_, CIRCLE_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(circle_->configure()); + + // Check that "radius" and "polygon_sub_topic" parameters are not set after configuring + ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "radius")); + ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "polygon_sub_topic")); +} + +TEST_F(Tester, testPolygonTopicUpdate) +{ + 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()); + + // Publish correct polygon and make sure 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, testCircleTopicUpdate) +{ + createCircle("stop", false); + ASSERT_FALSE(circle_->isShapeSet()); + + // Publish radius and make sure that it was set correctly + test_node_->publishRadius(); + ASSERT_TRUE(waitRadius(500ms)); + EXPECT_NEAR(circle_->getRadius(), CIRCLE_RADIUS, EPSILON); + EXPECT_NEAR(circle_->getRadiusSquared(), CIRCLE_RADIUS * CIRCLE_RADIUS, 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(); + + std::vector footprint; + ASSERT_TRUE(waitFootprint(500ms, footprint)); + + ASSERT_TRUE(polygon_->isShapeSet()); + ASSERT_EQ(footprint.size(), 4u); + EXPECT_NEAR(footprint[0].x, SQUARE_POLYGON[0], EPSILON); + EXPECT_NEAR(footprint[0].y, SQUARE_POLYGON[1], EPSILON); + EXPECT_NEAR(footprint[1].x, SQUARE_POLYGON[2], EPSILON); + EXPECT_NEAR(footprint[1].y, SQUARE_POLYGON[3], EPSILON); + EXPECT_NEAR(footprint[2].x, SQUARE_POLYGON[4], EPSILON); + EXPECT_NEAR(footprint[2].y, SQUARE_POLYGON[5], EPSILON); + EXPECT_NEAR(footprint[3].x, SQUARE_POLYGON[6], EPSILON); + EXPECT_NEAR(footprint[3].y, SQUARE_POLYGON[7], EPSILON); +} + +TEST_F(Tester, testPolygonGetPointsInside) +{ + createPolygon("stop", true); + + std::vector points; + + // Out of boundaries points + points.push_back({1.0, 0.0}); + points.push_back({0.0, 1.0}); + points.push_back({-1.0, 0.0}); + points.push_back({0.0, -1.0}); + ASSERT_EQ(polygon_->getPointsInside(points), 0); + + // Add one point inside + points.push_back({-0.1, 0.3}); + ASSERT_EQ(polygon_->getPointsInside(points), 1); +} + +TEST_F(Tester, testPolygonGetPointsInsideEdge) +{ + // Test for checking edge cases in raytracing algorithm. + // All points are lie on the edge lines parallel to OX, where the raytracing takes place. + setCommonParameters(POLYGON_NAME, "stop"); + setPolygonParameters(ARBITRARY_POLYGON_STR, true); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + + std::vector points; + + // Out of boundaries points + points.push_back({-2.0, -1.0}); + points.push_back({-2.0, 0.0}); + points.push_back({-2.0, 1.0}); + points.push_back({3.0, -1.0}); + points.push_back({3.0, 0.0}); + points.push_back({3.0, 1.0}); + ASSERT_EQ(polygon_->getPointsInside(points), 0); + + // Add one point inside + points.push_back({0.0, 0.0}); + ASSERT_EQ(polygon_->getPointsInside(points), 1); +} + +TEST_F(Tester, testCircleGetPointsInside) +{ + createCircle("stop", true); + + std::vector points; + // Point out of radius + points.push_back({1.0, 0.0}); + ASSERT_EQ(circle_->getPointsInside(points), 0); + + // Add one point inside + points.push_back({-0.1, 0.3}); + ASSERT_EQ(circle_->getPointsInside(points), 1); +} + +TEST_F(Tester, testPolygonGetCollisionTime) +{ + createPolygon("approach", false); + + // Set footprint for Polygon + test_node_->publishFootprint(); + std::vector footprint; + ASSERT_TRUE(waitFootprint(500ms, footprint)); + ASSERT_EQ(footprint.size(), 4u); + + // Forward movement check + nav2_collision_monitor::Velocity vel{0.5, 0.0, 0.0}; // 0.5 m/s forward movement + // Two points 0.2 m ahead the footprint (0.5 m) + std::unordered_map> points_map; + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.7, -0.01}, {0.7, 0.01}}}); + // Collision is expected to be ~= 0.2 m / 0.5 m/s seconds + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP); + + // Backward movement check + vel = {-0.5, 0.0, 0.0}; // 0.5 m/s backward movement + // Two points 0.2 m behind the footprint (0.5 m) + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{-0.7, -0.01}, {-0.7, 0.01}}}); + // Collision is expected to be in ~= 0.2 m / 0.5 m/s seconds + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP); + + // Sideway movement check + vel = {0.0, 0.5, 0.0}; // 0.5 m/s sideway movement + // Two points 0.1 m ahead the footprint (0.5 m) + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{-0.01, 0.6}, {0.01, 0.6}}}); + // Collision is expected to be in ~= 0.1 m / 0.5 m/s seconds + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.2, SIMULATION_TIME_STEP); + + // Rotation check + vel = {0.0, 0.0, 1.0}; // 1.0 rad/s rotation + // ^ OX + // ' + // x'x <- 2 collision points + // ' + // ----------- <- robot footprint + // OY | ' | + // <...|....o....|... + // | ' | + // ----------- + // ' + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.49, -0.01}, {0.49, 0.01}}}); + // Collision is expected to be in ~= 45 degrees * M_PI / (180 degrees * 1.0 rad/s) seconds + double exp_res = 45 / 180 * M_PI; + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), exp_res, EPSILON); + + // Two points are already inside footprint + vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement + // Two points inside + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.1, -0.01}, {0.1, 0.01}}}); + // Collision already appeared: collision time should be 0 + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.0, EPSILON); + + // All points are out of simulation prediction + vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement + // Two points 0.6 m ahead the footprint (0.5 m) + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{1.1, -0.01}, {1.1, 0.01}}}); + // There is no collision: return value should be negative + EXPECT_LT(polygon_->getCollisionTime(points_map, vel), 0.0); +} + +TEST_F(Tester, testPolygonPublish) +{ + createPolygon("stop", true); + polygon_->publish(); + geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received = + test_node_->waitPolygonReceived(500ms); + + ASSERT_NE(polygon_received, nullptr); + ASSERT_EQ(polygon_received->polygon.points.size(), 4u); + EXPECT_NEAR(polygon_received->polygon.points[0].x, SQUARE_POLYGON[0], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[0].y, SQUARE_POLYGON[1], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[1].x, SQUARE_POLYGON[2], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[1].y, SQUARE_POLYGON[3], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[2].x, SQUARE_POLYGON[4], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[2].y, SQUARE_POLYGON[5], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[3].x, SQUARE_POLYGON[6], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[3].y, SQUARE_POLYGON[7], EPSILON); + + polygon_->deactivate(); +} + +TEST_F(Tester, testPolygonDefaultVisualize) +{ + // Use default parameters, visualize should be false by-default + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); + std::vector observation_sources = {OBSERVATION_SOURCE_NAME}; + test_node_->declare_parameter("observation_sources", rclcpp::ParameterValue(observation_sources)); + test_node_->set_parameter(rclcpp::Parameter("observation_sources", observation_sources)); + setPolygonParameters(SQUARE_POLYGON_STR, true); + + // Create new polygon + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + polygon_->activate(); + + // Try to publish polygon + polygon_->publish(); + + // Wait for polygon: it should not be published + ASSERT_EQ(test_node_->waitPolygonReceived(100ms), nullptr); +} + +TEST_F(Tester, testPolygonInvalidPointsString) +{ + setCommonParameters(POLYGON_NAME, "stop"); + + // Invalid points + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INVALID_POINTS_STR)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INVALID_POINTS_STR)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + +TEST_F(Tester, testPolygonSourceDefaultAssociation) +{ + // By default, a polygon uses all observation sources + std::vector all_sources = {"source_1", "source_2", "source_3"}; + setCommonParameters(POLYGON_NAME, "stop", all_sources); // no polygon sources names specified + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + ASSERT_EQ(polygon_->getSourcesNames(), all_sources); +} + +TEST_F(Tester, testPolygonSourceInvalidAssociation) +{ + // If a source is not defined as observation source, polygon cannot use it: config should fail + setCommonParameters( + POLYGON_NAME, "stop", {"source_1", "source_2", "source_3"}, {"source_1", "source_4"}); + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + +TEST_F(Tester, testPolygonSourceAssociation) +{ + // Checks that, if declared, only the specific sources associated to polygon are saved + std::vector poly_sources = {"source_1", "source_3"}; + setCommonParameters(POLYGON_NAME, "stop", {"source_1", "source_2", "source_3"}, poly_sources); + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + ASSERT_EQ(polygon_->getSourcesNames(), poly_sources); +} + +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; +} diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp new file mode 100644 index 00000000000..0f3cb195064 --- /dev/null +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -0,0 +1,757 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/range.hpp" +#include "sensor_msgs/point_cloud2_iterator.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/scan.hpp" +#include "nav2_collision_monitor/pointcloud.hpp" +#include "nav2_collision_monitor/range.hpp" +#include "nav2_collision_monitor/polygon_source.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 SOURCE_FRAME_ID[]{"base_source"}; +static const char GLOBAL_FRAME_ID[]{"odom"}; +static const char SCAN_NAME[]{"LaserScan"}; +static const char SCAN_TOPIC[]{"scan"}; +static const char POINTCLOUD_NAME[]{"PointCloud"}; +static const char POINTCLOUD_TOPIC[]{"pointcloud"}; +static const char RANGE_NAME[]{"Range"}; +static const char RANGE_TOPIC[]{"range"}; +static const char POLYGON_NAME[]{"Polygon"}; +static const char POLYGON_TOPIC[]{"polygon"}; +static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; +static const rclcpp::Duration DATA_TIMEOUT{rclcpp::Duration::from_seconds(5.0)}; + +class TestNode : public nav2_util::LifecycleNode +{ +public: + TestNode() + : nav2_util::LifecycleNode("test_node") + { + } + + ~TestNode() + { + scan_pub_.reset(); + pointcloud_pub_.reset(); + range_pub_.reset(); + polygon_pub_.reset(); + } + + void publishScan(const rclcpp::Time & stamp, const double range) + { + scan_pub_ = this->create_publisher( + SCAN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->angle_min = 0.0; + msg->angle_max = 2 * M_PI; + msg->angle_increment = M_PI / 2; + msg->time_increment = 0.0; + msg->scan_time = 0.0; + msg->range_min = 0.1; + msg->range_max = 1.1; + std::vector ranges(4, range); + msg->ranges = ranges; + + scan_pub_->publish(std::move(msg)); + } + + void publishPointCloud(const rclcpp::Time & stamp) + { + pointcloud_pub_ = this->create_publisher( + POINTCLOUD_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*msg); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + modifier.resize(3); + + sensor_msgs::PointCloud2Iterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); + + // Point 0: (0.5, 0.5, 0.2) + *iter_x = 0.5; + *iter_y = 0.5; + *iter_z = 0.2; + ++iter_x; ++iter_y; ++iter_z; + + // Point 1: (-0.5, -0.5, 0.3) + *iter_x = -0.5; + *iter_y = -0.5; + *iter_z = 0.3; + ++iter_x; ++iter_y; ++iter_z; + + // Point 2: (1.0, 1.0, 10.0) + *iter_x = 1.0; + *iter_y = 1.0; + *iter_z = 10.0; + + pointcloud_pub_->publish(std::move(msg)); + } + + void publishRange(const rclcpp::Time & stamp, const double range) + { + range_pub_ = this->create_publisher( + RANGE_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->radiation_type = 0; + msg->field_of_view = M_PI / 10; + msg->min_range = 0.1; + msg->max_range = 1.1; + msg->range = range; + + range_pub_->publish(std::move(msg)); + } + + void publishPolygon(const rclcpp::Time & stamp) + { + polygon_pub_ = this->create_publisher( + POLYGON_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + geometry_msgs::msg::Point32 point; + point.x = 1.0; + point.y = -1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + point.x = 1.0; + point.y = 1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + point.x = -1.0; + point.y = 1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + point.x = -1.0; + point.y = -1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + msg->polygon.id = 1u; + polygon_pub_->publish(std::move(msg)); + } + +private: + rclcpp::Publisher::SharedPtr scan_pub_; + rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr range_pub_; + rclcpp::Publisher::SharedPtr polygon_pub_; +}; // TestNode + +class ScanWrapper : public nav2_collision_monitor::Scan +{ +public: + ScanWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & data_timeout, + const bool base_shift_correction) + : nav2_collision_monitor::Scan( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, data_timeout, base_shift_correction) + {} + + bool dataReceived() const + { + return data_ != nullptr; + } +}; // ScanWrapper + +class PointCloudWrapper : public nav2_collision_monitor::PointCloud +{ +public: + PointCloudWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & data_timeout, + const bool base_shift_correction) + : nav2_collision_monitor::PointCloud( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, data_timeout, base_shift_correction) + {} + + bool dataReceived() const + { + return data_ != nullptr; + } +}; // PointCloudWrapper + +class RangeWrapper : public nav2_collision_monitor::Range +{ +public: + RangeWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & data_timeout, + const bool base_shift_correction) + : nav2_collision_monitor::Range( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, data_timeout, base_shift_correction) + {} + + bool dataReceived() const + { + return data_ != nullptr; + } +}; // RangeWrapper + +class PolygonWrapper : public nav2_collision_monitor::PolygonSource +{ +public: + PolygonWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & data_timeout, + const bool base_shift_correction) + : nav2_collision_monitor::PolygonSource( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, data_timeout, base_shift_correction) + {} + + bool dataReceived() const + { + return data_.size() > 0; + } +}; // PolygonWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + +protected: + // Data sources creation routine + void createSources(const bool base_shift_correction = true); + + // Setting TF chains + void sendTransforms(const rclcpp::Time & stamp); + + // Data sources working routines + bool waitScan(const std::chrono::nanoseconds & timeout); + bool waitPointCloud(const std::chrono::nanoseconds & timeout); + bool waitRange(const std::chrono::nanoseconds & timeout); + bool waitPolygon(const std::chrono::nanoseconds & timeout); + void checkScan(const std::vector & data); + void checkPointCloud(const std::vector & data); + void checkRange(const std::vector & data); + void checkPolygon(const std::vector & data); + + std::shared_ptr test_node_; + std::shared_ptr scan_; + std::shared_ptr pointcloud_; + std::shared_ptr range_; + std::shared_ptr polygon_; + +private: + 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() +{ + scan_.reset(); + pointcloud_.reset(); + range_.reset(); + + test_node_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::createSources(const bool base_shift_correction) +{ + // Create Scan object + test_node_->declare_parameter( + std::string(SCAN_NAME) + ".topic", rclcpp::ParameterValue(SCAN_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(SCAN_NAME) + ".topic", SCAN_TOPIC)); + + scan_ = std::make_shared( + test_node_, SCAN_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); + scan_->configure(); + + // Create PointCloud object + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".topic", rclcpp::ParameterValue(POINTCLOUD_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".topic", POINTCLOUD_TOPIC)); + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".min_height", rclcpp::ParameterValue(0.1)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".min_height", 0.1)); + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".max_height", rclcpp::ParameterValue(1.0)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".max_height", 1.0)); + + pointcloud_ = std::make_shared( + test_node_, POINTCLOUD_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); + pointcloud_->configure(); + + // Create Range object + test_node_->declare_parameter( + std::string(RANGE_NAME) + ".topic", rclcpp::ParameterValue(RANGE_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(RANGE_NAME) + ".topic", RANGE_TOPIC)); + + test_node_->declare_parameter( + std::string(RANGE_NAME) + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 199)); + + range_ = std::make_shared( + test_node_, RANGE_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); + range_->configure(); + + // Create Polygon object + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".topic", rclcpp::ParameterValue(POLYGON_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".topic", POLYGON_TOPIC)); + + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".sampling_distance", rclcpp::ParameterValue(0.1)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); + polygon_->configure(); +} + +void Tester::sendTransforms(const rclcpp::Time & stamp) +{ + std::shared_ptr tf_broadcaster = + std::make_shared(test_node_); + + geometry_msgs::msg::TransformStamped transform; + + // base_frame -> source_frame transform + transform.header.frame_id = BASE_FRAME_ID; + transform.child_frame_id = SOURCE_FRAME_ID; + + transform.header.stamp = stamp; + transform.transform.translation.x = 0.1; + transform.transform.translation.y = 0.1; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); + + // global_frame -> base_frame transform + transform.header.frame_id = GLOBAL_FRAME_ID; + transform.child_frame_id = BASE_FRAME_ID; + + transform.transform.translation.x = 0.0; + transform.transform.translation.y = 0.0; + + tf_broadcaster->sendTransform(transform); +} + +bool Tester::waitScan(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (scan_->dataReceived()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitPointCloud(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (pointcloud_->dataReceived()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitRange(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (range_->dataReceived()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitPolygon(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (polygon_->dataReceived()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +void Tester::checkScan(const std::vector & data) +{ + ASSERT_EQ(data.size(), 4u); + + // Point 0: (1.0 + 0.1, 0.0 + 0.1) + EXPECT_NEAR(data[0].x, 1.1, EPSILON); + EXPECT_NEAR(data[0].y, 0.1, EPSILON); + + // Point 1: (0.0 + 0.1, 1.0 + 0.1) + EXPECT_NEAR(data[1].x, 0.1, EPSILON); + EXPECT_NEAR(data[1].y, 1.1, EPSILON); + + // Point 2: (-1.0 + 0.1, 0.0 + 0.1) + EXPECT_NEAR(data[2].x, -0.9, EPSILON); + EXPECT_NEAR(data[2].y, 0.1, EPSILON); + + // Point 3: (0.0 + 0.1, -1.0 + 0.1) + EXPECT_NEAR(data[3].x, 0.1, EPSILON); + EXPECT_NEAR(data[3].y, -0.9, EPSILON); +} + +void Tester::checkPointCloud(const std::vector & data) +{ + ASSERT_EQ(data.size(), 2u); + + // Point 0: (0.5 + 0.1, 0.5 + 0.1) + EXPECT_NEAR(data[0].x, 0.6, EPSILON); + EXPECT_NEAR(data[0].y, 0.6, EPSILON); + + // Point 1: (-0.5 + 0.1, -0.5 + 0.1) + EXPECT_NEAR(data[1].x, -0.4, EPSILON); + EXPECT_NEAR(data[1].y, -0.4, EPSILON); + + // Point 2 should be out of scope by height +} + +void Tester::checkRange(const std::vector & data) +{ + ASSERT_EQ(data.size(), 21u); + + const double angle_increment = M_PI / 199; + double angle = -M_PI / (10 * 2); + int i; + for (i = 0; i < 199 / 10 + 1; i++) { + ASSERT_NEAR(data[i].x, 1.0 * std::cos(angle) + 0.1, EPSILON); + ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON); + angle += angle_increment; + } + // Check for the latest FoW/2 point + angle = M_PI / (10 * 2); + ASSERT_NEAR(data[i].x, 1.0 * std::cos(angle) + 0.1, EPSILON); + ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON); +} + +void Tester::checkPolygon(const std::vector & data) +{ + ASSERT_EQ(data.size(), 84u); +} + +TEST_F(Tester, testGetData) +{ + rclcpp::Time curr_time = test_node_->now(); + + createSources(); + + sendTransforms(curr_time); + + // Publish data for sources + test_node_->publishScan(curr_time, 1.0); + test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 1.0); + test_node_->publishPolygon(curr_time); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); + + // Check Scan data + std::vector data; + scan_->getData(curr_time, data); + checkScan(data); + + // Check Pointcloud data + data.clear(); + pointcloud_->getData(curr_time, data); + checkPointCloud(data); + + // Check Range data + data.clear(); + range_->getData(curr_time, data); + checkRange(data); + + // Check Polygon data + data.clear(); + polygon_->getData(curr_time, data); + checkPolygon(data); +} + +TEST_F(Tester, testGetOutdatedData) +{ + rclcpp::Time curr_time = test_node_->now(); + + createSources(); + + sendTransforms(curr_time); + + // Publish outdated data for sources + test_node_->publishScan(curr_time - DATA_TIMEOUT - 1s, 1.0); + test_node_->publishPointCloud(curr_time - DATA_TIMEOUT - 1s); + test_node_->publishRange(curr_time - DATA_TIMEOUT - 1s, 1.0); + test_node_->publishPolygon(curr_time - DATA_TIMEOUT - 1s); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); + + // Scan data should be empty + std::vector data; + scan_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Pointcloud data should be empty + pointcloud_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Polygon data should be empty + polygon_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); +} + +TEST_F(Tester, testIncorrectFrameData) +{ + rclcpp::Time curr_time = test_node_->now(); + + createSources(); + + // Send incorrect transform + sendTransforms(curr_time - 1s); + + // Publish data for sources + test_node_->publishScan(curr_time, 1.0); + test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 1.0); + test_node_->publishPolygon(curr_time); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); + + // Scan data should be empty + std::vector data; + scan_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Pointcloud data should be empty + pointcloud_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Polygon data should be empty + polygon_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); +} + +TEST_F(Tester, testIncorrectData) +{ + rclcpp::Time curr_time = test_node_->now(); + + createSources(); + + sendTransforms(curr_time); + + // Publish data for sources + test_node_->publishScan(curr_time, 2.0); + test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 2.0); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitRange(500ms)); + + // Scan data should be empty + std::vector data; + scan_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); +} + +TEST_F(Tester, testIgnoreTimeShift) +{ + rclcpp::Time curr_time = test_node_->now(); + + createSources(false); + + // Send incorrect transform + sendTransforms(curr_time - 1s); + + // Publish data for sources + test_node_->publishScan(curr_time, 1.0); + test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 1.0); + test_node_->publishPolygon(curr_time); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); + + // Scan data should be consistent + std::vector data; + scan_->getData(curr_time, data); + checkScan(data); + + // Pointcloud data should be consistent + data.clear(); + pointcloud_->getData(curr_time, data); + checkPointCloud(data); + + // Range data should be consistent + data.clear(); + range_->getData(curr_time, data); + checkRange(data); + + // Polygon data should be consistent + data.clear(); + polygon_->getData(curr_time, data); + checkPolygon(data); +} + +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; +} 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..7d57cc00de8 --- /dev/null +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -0,0 +1,582 @@ +// 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 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)); + + std::vector default_observation_sources = {"source"}; + test_node_->declare_parameter( + "observation_sources", rclcpp::ParameterValue(default_observation_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", default_observation_sources)); +} + +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; +} diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 7a8d77903fb..2865ac2f02d 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -24,6 +24,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" + "msg/PolygonInstance.msg" + "msg/PolygonInstanceStamped.msg" "msg/MissedWaypoint.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" diff --git a/nav2_msgs/msg/PolygonInstance.msg b/nav2_msgs/msg/PolygonInstance.msg new file mode 100644 index 00000000000..cb823162729 --- /dev/null +++ b/nav2_msgs/msg/PolygonInstance.msg @@ -0,0 +1,5 @@ +# A specification of a polygon where the first and last points are assumed to be connected +# It includes a unique identification field for disambiguating multiple instances + +geometry_msgs/Polygon polygon +int64 id \ No newline at end of file diff --git a/nav2_msgs/msg/PolygonInstanceStamped.msg b/nav2_msgs/msg/PolygonInstanceStamped.msg new file mode 100644 index 00000000000..b9fb1fc23b6 --- /dev/null +++ b/nav2_msgs/msg/PolygonInstanceStamped.msg @@ -0,0 +1,5 @@ +# This represents a Polygon with reference coordinate frame and timestamp +# It includes a unique identification field for disambiguating multiple instances + +std_msgs/Header header +nav2_msgs/PolygonInstance polygon \ 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 00000000000..f232cf30274 --- /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_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index a639a0e59e9..104966e2198 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -8,6 +8,7 @@ add_library(${library_name} SHARED robot_utils.cpp node_thread.cpp odometry_utils.cpp + array_parser.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_util/src/array_parser.cpp b/nav2_util/src/array_parser.cpp new file mode 100644 index 00000000000..cfe2f699145 --- /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