From 6f06e1311e9ac355549f9cd4d15524e9a9792c97 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 14 Aug 2023 14:12:57 +0200 Subject: [PATCH] nav2_collision_monitor: collision detector (#3500) Signed-off-by: enricosutera --- .circleci/config.yml | 6 +- nav2_collision_monitor/CMakeLists.txt | 53 +- nav2_collision_monitor/README.md | 30 +- .../collision_detector_node.hpp | 159 ++++ .../collision_monitor_node.hpp | 6 +- .../launch/collision_detector_node.launch.py | 150 ++++ .../params/collision_detector_params.yaml | 25 + .../src/collision_detector_main.cpp | 29 + .../src/collision_detector_node.cpp | 340 +++++++++ .../{main.cpp => collision_monitor_main.cpp} | 0 .../src/collision_monitor_node.cpp | 5 +- nav2_collision_monitor/src/polygon.cpp | 6 +- nav2_collision_monitor/test/CMakeLists.txt | 16 +- .../test/collision_detector_node_test.cpp | 695 ++++++++++++++++++ .../test/collision_monitor_node_test.cpp | 1 - nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/msg/CollisionDetectorState.msg | 4 + 17 files changed, 1493 insertions(+), 33 deletions(-) create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp create mode 100644 nav2_collision_monitor/launch/collision_detector_node.launch.py create mode 100644 nav2_collision_monitor/params/collision_detector_params.yaml create mode 100644 nav2_collision_monitor/src/collision_detector_main.cpp create mode 100644 nav2_collision_monitor/src/collision_detector_node.cpp rename nav2_collision_monitor/src/{main.cpp => collision_monitor_main.cpp} (100%) create mode 100644 nav2_collision_monitor/test/collision_detector_node_test.cpp create mode 100644 nav2_msgs/msg/CollisionDetectorState.msg diff --git a/.circleci/config.yml b/.circleci/config.yml index f0eda5648b..9d3e0f490d 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v15\ + - "<< parameters.key >>-v16\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v15\ + - "<< parameters.key >>-v16\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v15\ + key: "<< parameters.key >>-v16\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 2cc7b19c73..51cafe71dd 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -39,10 +39,12 @@ set(dependencies nav2_msgs ) -set(executable_name collision_monitor) -set(library_name ${executable_name}_core) +set(monitor_executable_name collision_monitor) +set(detector_executable_name collision_detector) +set(monitor_library_name ${monitor_executable_name}_core) +set(detector_library_name ${detector_executable_name}_core) -add_library(${library_name} SHARED +add_library(${monitor_library_name} SHARED src/collision_monitor_node.cpp src/polygon.cpp src/circle.cpp @@ -52,34 +54,59 @@ add_library(${library_name} SHARED src/range.cpp src/kinematics.cpp ) +add_library(${detector_library_name} SHARED + src/collision_detector_node.cpp + src/polygon.cpp + src/circle.cpp + src/source.cpp + src/scan.cpp + src/pointcloud.cpp + src/range.cpp + src/kinematics.cpp +) -add_executable(${executable_name} - src/main.cpp +add_executable(${monitor_executable_name} + src/collision_monitor_main.cpp +) +add_executable(${detector_executable_name} + src/collision_detector_main.cpp ) -ament_target_dependencies(${library_name} +ament_target_dependencies(${monitor_library_name} + ${dependencies} +) +ament_target_dependencies(${detector_library_name} ${dependencies} ) -target_link_libraries(${executable_name} - ${library_name} +target_link_libraries(${monitor_executable_name} + ${monitor_library_name} +) +target_link_libraries(${detector_executable_name} + ${detector_library_name} +) + +ament_target_dependencies(${monitor_executable_name} + ${dependencies} ) -ament_target_dependencies(${executable_name} +ament_target_dependencies(${detector_executable_name} ${dependencies} ) -rclcpp_components_register_nodes(${library_name} "nav2_collision_monitor::CollisionMonitor") +rclcpp_components_register_nodes(${monitor_library_name} "nav2_collision_monitor::CollisionMonitor") + +rclcpp_components_register_nodes(${detector_library_name} "nav2_collision_monitor::CollisionDetector") ### Install ### -install(TARGETS ${library_name} +install(TARGETS ${monitor_library_name} ${detector_library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(TARGETS ${executable_name} +install(TARGETS ${monitor_executable_name} ${detector_executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) @@ -106,7 +133,7 @@ endif() ### Ament stuff ### ament_export_include_directories(include) -ament_export_libraries(${library_name}) +ament_export_libraries(${monitor_library_name} ${detector_library_name}) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 134a52cff6..864b28bbe4 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,7 @@ The costmaps / trajectory planners will handle most situations, but this is to h ![polygons.png](doc/polygons.png) -## Features +### 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. @@ -23,7 +25,6 @@ The following models of safety behaviors are employed by Collision Monitor: * **Stop model**: Define a zone and a point threshold. If more that `N` obstacle points appear inside this area, stop the robot until the obstacles will disappear. * **Slowdown model**: Define a zone around the robot and slow the maximum speed for a `%S` percent, if more than `N` points will appear inside the area. -* **Limit model**: Define a zone around the robot and clamp the maximum speed below a fixed value, if more than `N` points will appear inside the area. * **Approach model**: Using the current robot speed, estimate the time to collision to sensor data. If the time is less than `M` seconds (0.5, 2, 5, etc...), the robot will slow such that it is now at least `M` seconds to collision. The effect here would be to keep the robot always `M` seconds from any collision. The zones around the robot can take the following shapes: @@ -38,20 +39,19 @@ 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. ![HLD.png](doc/HLD.png) - -## Configuration +### Configuration Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages. -## Metrics +### 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). @@ -66,3 +66,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/configuring-collision-detector.html). \ No newline at end of file diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp new file mode 100644 index 0000000000..c4c5906b22 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -0,0 +1,159 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// Copyright (c) 2023 Pixel Robotics GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_ +#define NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "tf2/time.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_msgs/msg/collision_detector_state.hpp" + +#include "nav2_collision_monitor/types.hpp" +#include "nav2_collision_monitor/polygon.hpp" +#include "nav2_collision_monitor/circle.hpp" +#include "nav2_collision_monitor/source.hpp" +#include "nav2_collision_monitor/scan.hpp" +#include "nav2_collision_monitor/pointcloud.hpp" +#include "nav2_collision_monitor/range.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Collision Monitor ROS2 node + */ +class CollisionDetector : public nav2_util::LifecycleNode +{ +public: + /** + * @brief Constructor for the nav2_collision_monitor::CollisionDetector + * @param options Additional options to control creation of the node. + */ + explicit CollisionDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /** + * @brief Destructor for the nav2_collision_monitor::CollisionDetector + */ + ~CollisionDetector(); + +protected: + /** + * @brief: Initializes and obtains ROS-parameters, creates main subscribers and publishers, + * creates polygons and data sources objects + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Activates LifecyclePublishers, polygons and main processor, creates bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Resets all subscribers/publishers, polygons/data sources arrays + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called in shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + +protected: + /** + * @brief Supporting routine obtaining all ROS-parameters + * @return True if all parameters were obtained or false in failure case + */ + bool getParameters(); + /** + * @brief Supporting routine creating and configuring all polygons + * @param base_frame_id Robot base frame ID + * @param transform_tolerance Transform tolerance + * @return True if all polygons were configured successfully or false in failure case + */ + bool configurePolygons( + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance); + /** + * @brief Supporting routine creating and configuring all data sources + * @param base_frame_id Robot base frame ID + * @param odom_frame_id Odometry frame ID. Used as global frame to get + * source->base time interpolated transform. + * @param transform_tolerance Transform tolerance + * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time + * @return True if all sources were configured successfully or false in failure case + */ + bool configureSources( + const std::string & base_frame_id, + const std::string & odom_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); + + /** + * @brief Main processing routine + */ + void process(); + + /** + * @brief Polygons publishing routine. Made for visualization. + */ + void publishPolygons() const; + + // ----- Variables ----- + + /// @brief TF buffer + std::shared_ptr tf_buffer_; + /// @brief TF listener + std::shared_ptr tf_listener_; + + /// @brief Polygons array + std::vector> polygons_; + /// @brief Data sources array + std::vector> sources_; + + /// @brief collision monitor state publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + state_pub_; + /// @brief timer that runs actions + rclcpp::TimerBase::SharedPtr timer_; + + /// @brief main loop frequency + double frequency_; +}; // class CollisionDetector + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 52934fc12d..0383dee363 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 @@ -47,12 +47,12 @@ class CollisionMonitor : public nav2_util::LifecycleNode { public: /** - * @brief Constructor for the nav2_collision_safery::CollisionMonitor + * @brief Constructor for the nav2_collision_monitor::CollisionMonitor * @param options Additional options to control creation of the node. */ explicit CollisionMonitor(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** - * @brief Destructor for the nav2_collision_safery::CollisionMonitor + * @brief Destructor for the nav2_collision_monitor::CollisionMonitor */ ~CollisionMonitor(); @@ -126,7 +126,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @brief Supporting routine creating and configuring all data sources * @param base_frame_id Robot base frame ID * @param odom_frame_id Odometry frame ID. Used as global frame to get - * source->base time inerpolated transform. + * source->base time interpolated transform. * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py new file mode 100644 index 0000000000..ccd58c2e15 --- /dev/null +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 + +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +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.actions import PushRosNamespace +from launch_ros.descriptions import ComposableNode +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Environment + package_dir = get_package_share_directory('nav2_collision_monitor') + + # Constant parameters + lifecycle_nodes = ['collision_detector'] + 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') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='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') + + declare_use_composition_cmd = DeclareLaunchArgument( + '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') + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + 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), + 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), + 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) + ] + ) + + 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_detector', + 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) + ], + ) + ] + ) + + ld = LaunchDescription() + + # Launch arguments + 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(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml new file mode 100644 index 0000000000..218ce45239 --- /dev/null +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -0,0 +1,25 @@ +collision_detector: + ros__parameters: + frequency: 10.0 + base_frame_id: "base_footprint" + odom_frame_id: "odom" + transform_tolerance: 0.5 + source_timeout: 5.0 + base_shift_correction: True + polygons: ["PolygonFront"] + PolygonFront: + type: "polygon" + 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" + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + pointcloud: + type: "pointcloud" + topic: "/intel_realsense_r200_depth/points" + min_height: 0.1 + max_height: 0.5 diff --git a/nav2_collision_monitor/src/collision_detector_main.cpp b/nav2_collision_monitor/src/collision_detector_main.cpp new file mode 100644 index 0000000000..c4ef21b7d6 --- /dev/null +++ b/nav2_collision_monitor/src/collision_detector_main.cpp @@ -0,0 +1,29 @@ +// 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 "rclcpp/rclcpp.hpp" + +#include "nav2_collision_monitor/collision_detector_node.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp new file mode 100644 index 0000000000..e12d9795db --- /dev/null +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -0,0 +1,340 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// Copyright (c) 2023 Pixel Robotics GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_collision_monitor/collision_detector_node.hpp" + +#include +#include +#include + +#include "tf2_ros/create_timer_ros.h" + +#include "nav2_util/node_utils.hpp" + +using namespace std::chrono_literals; + +namespace nav2_collision_monitor +{ + +CollisionDetector::CollisionDetector(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("collision_detector", "", options) +{ +} + +CollisionDetector::~CollisionDetector() +{ + polygons_.clear(); + sources_.clear(); +} + +nav2_util::CallbackReturn +CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(this->get_clock()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), + this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + + state_pub_ = this->create_publisher( + "collision_detector_state", rclcpp::SystemDefaultsQoS()); + + // Obtaining ROS parameters + if (!getParameters()) { + return nav2_util::CallbackReturn::FAILURE; + } + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + // Activating lifecycle publisher + state_pub_->on_activate(); + + // Activating polygons + for (std::shared_ptr polygon : polygons_) { + polygon->activate(); + } + + // Creating timer + timer_ = this->create_timer( + std::chrono::duration{1.0 / frequency_}, + std::bind(&CollisionDetector::process, this)); + + // Creating bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + // Resetting timer + timer_.reset(); + + // Deactivating lifecycle publishers + state_pub_->on_deactivate(); + + // Deactivating polygons + for (std::shared_ptr polygon : polygons_) { + polygon->deactivate(); + } + + // Destroying bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionDetector::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + state_pub_.reset(); + + polygons_.clear(); + sources_.clear(); + + tf_listener_.reset(); + tf_buffer_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionDetector::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} + +bool CollisionDetector::getParameters() +{ + std::string base_frame_id, odom_frame_id; + tf2::Duration transform_tolerance; + rclcpp::Duration source_timeout(2.0, 0.0); + + auto node = shared_from_this(); + + nav2_util::declare_parameter_if_not_declared( + node, "frequency", rclcpp::ParameterValue(10.0)); + frequency_ = get_parameter("frequency").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, "base_frame_id", rclcpp::ParameterValue("base_footprint")); + base_frame_id = get_parameter("base_frame_id").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, "odom_frame_id", rclcpp::ParameterValue("odom")); + odom_frame_id = get_parameter("odom_frame_id").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, "transform_tolerance", rclcpp::ParameterValue(0.1)); + transform_tolerance = + tf2::durationFromSec(get_parameter("transform_tolerance").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "source_timeout", rclcpp::ParameterValue(2.0)); + source_timeout = + rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "base_shift_correction", rclcpp::ParameterValue(true)); + const bool base_shift_correction = + get_parameter("base_shift_correction").as_bool(); + + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + + if (!configureSources( + base_frame_id, odom_frame_id, transform_tolerance, source_timeout, + base_shift_correction)) + { + return false; + } + + return true; +} + +bool CollisionDetector::configurePolygons( + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance) +{ + try { + auto node = shared_from_this(); + + // Leave it to be not initialized: to intentionally cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, "polygons", rclcpp::PARAMETER_STRING_ARRAY); + std::vector polygon_names = get_parameter("polygons").as_string_array(); + for (std::string polygon_name : polygon_names) { + // Leave it not initialized: the will cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, polygon_name + ".type", rclcpp::PARAMETER_STRING); + const std::string polygon_type = get_parameter(polygon_name + ".type").as_string(); + + if (polygon_type == "polygon") { + polygons_.push_back( + std::make_shared( + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + } else if (polygon_type == "circle") { + polygons_.push_back( + std::make_shared( + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + } else { // Error if something else + RCLCPP_ERROR( + get_logger(), + "[%s]: Unknown polygon type: %s", + polygon_name.c_str(), polygon_type.c_str()); + return false; + } + + // Configure last added polygon + if (!polygons_.back()->configure()) { + return false; + } + + // warn if the added polygon's action_type_ is not different than "none" + auto action_type = polygons_.back()->getActionType(); + if (action_type != DO_NOTHING) { + RCLCPP_ERROR( + get_logger(), + "[%s]: The action_type of the polygon is different than \"none\" which is " + "not supported in the collision detector.", + polygon_name.c_str()); + return false; + } + + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); + return false; + } + + return true; +} + +bool CollisionDetector::configureSources( + const std::string & base_frame_id, + const std::string & odom_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) +{ + try { + auto node = shared_from_this(); + + // Leave it to be not initialized to intentionally cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY); + std::vector source_names = get_parameter("observation_sources").as_string_array(); + for (std::string source_name : source_names) { + nav2_util::declare_parameter_if_not_declared( + node, source_name + ".type", + rclcpp::ParameterValue("scan")); // Laser scanner by default + const std::string source_type = get_parameter(source_name + ".type").as_string(); + + if (source_type == "scan") { + std::shared_ptr s = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + + s->configure(); + + sources_.push_back(s); + } else if (source_type == "pointcloud") { + std::shared_ptr p = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + + p->configure(); + + sources_.push_back(p); + } else if (source_type == "range") { + std::shared_ptr r = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + + r->configure(); + + sources_.push_back(r); + } else { // Error if something else + RCLCPP_ERROR( + get_logger(), + "[%s]: Unknown source type: %s", + source_name.c_str(), source_type.c_str()); + return false; + } + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); + return false; + } + + return true; +} + +void CollisionDetector::process() +{ + // Current timestamp for all inner routines prolongation + rclcpp::Time curr_time = this->now(); + + // Points array collected from different data sources in a robot base frame + std::vector collision_points; + + // Fill collision_points array from different data sources + for (std::shared_ptr source : sources_) { + source->getData(curr_time, collision_points); + } + + std::unique_ptr state_msg = + std::make_unique(); + + for (std::shared_ptr polygon : polygons_) { + state_msg->polygons.push_back(polygon->getName()); + state_msg->detections.push_back( + polygon->getPointsInside( + collision_points) > polygon->getMinPoints()); + } + + state_pub_->publish(std::move(state_msg)); + + // Publish polygons for better visualization + publishPolygons(); +} + +void CollisionDetector::publishPolygons() const +{ + for (std::shared_ptr polygon : polygons_) { + polygon->publish(); + } +} + +} // namespace nav2_collision_monitor + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_collision_monitor::CollisionDetector) diff --git a/nav2_collision_monitor/src/main.cpp b/nav2_collision_monitor/src/collision_monitor_main.cpp similarity index 100% rename from nav2_collision_monitor/src/main.cpp rename to nav2_collision_monitor/src/collision_monitor_main.cpp diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 2bae91e2a0..6b678c9c76 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -259,8 +259,9 @@ bool CollisionMonitor::configurePolygons( try { auto node = shared_from_this(); + // Leave it to be not initialized: to intentionally cause an error if it will not set nav2_util::declare_parameter_if_not_declared( - node, "polygons", rclcpp::ParameterValue(std::vector())); + node, "polygons", rclcpp::PARAMETER_STRING_ARRAY); std::vector polygon_names = get_parameter("polygons").as_string_array(); for (std::string polygon_name : polygon_names) { // Leave it not initialized: the will cause an error if it will not set @@ -448,7 +449,7 @@ bool CollisionMonitor::processStopSlowdownLimit( } } else { // Limit // Compute linear velocity - const double linear_vel = std::hypot(velocity.x, velocity.y); // absolute + const double linear_vel = std::hypot(velocity.x, velocity.y); // absolute Velocity safe_vel; double ratio = 1.0; if (linear_vel != 0.0) { diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 8ad9cb09df..8b7a3c7c00 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -295,6 +295,8 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) action_type_ = LIMIT; } else if (at_str == "approach") { action_type_ = APPROACH; + } else if (at_str == "none") { + action_type_ = DO_NOTHING; } else { // Error if something else RCLCPP_ERROR(logger_, "[%s]: Unknown action type: %s", polygon_name_.c_str(), at_str.c_str()); return false; @@ -420,7 +422,9 @@ bool Polygon::getParameters( polygon_name_.c_str()); } - if (action_type_ == STOP || action_type_ == SLOWDOWN || action_type_ == LIMIT) { + if (action_type_ == STOP || action_type_ == SLOWDOWN || action_type_ == LIMIT || + action_type_ == DO_NOTHING) + { // Dynamic polygon will be used nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING); diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt index 77870826b8..f2b4619bbd 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -4,7 +4,7 @@ ament_target_dependencies(kinematics_test ${dependencies} ) target_link_libraries(kinematics_test - ${library_name} + ${monitor_library_name} ) # Data sources test @@ -13,7 +13,7 @@ ament_target_dependencies(sources_test ${dependencies} ) target_link_libraries(sources_test - ${library_name} + ${monitor_library_name} ) # Polygon shapes test @@ -22,7 +22,7 @@ ament_target_dependencies(polygons_test ${dependencies} ) target_link_libraries(polygons_test - ${library_name} + ${monitor_library_name} ) # Collision Monitor node test @@ -31,5 +31,13 @@ ament_target_dependencies(collision_monitor_node_test ${dependencies} ) target_link_libraries(collision_monitor_node_test - ${library_name} + ${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 0000000000..3400f1d50d --- /dev/null +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -0,0 +1,695 @@ +// 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 "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 FOOTPRINT_TOPIC[]{"footprint"}; +static const char SCAN_NAME[]{"Scan"}; +static const char POINTCLOUD_NAME[]{"PointCloud"}; +static const char RANGE_NAME[]{"Range"}; +static const char STATE_TOPIC[]{"collision_detector_state"}; +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 +}; + +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); + 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); + +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::Subscription::SharedPtr state_sub_; + nav2_msgs::msg::CollisionDetectorState::SharedPtr state_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()); + + state_sub_ = cd_->create_subscription( + STATE_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::stateCallback, this, std::placeholders::_1)); +} + +Tester::~Tester() +{ + scan_pub_.reset(); + pointcloud_pub_.reset(); + range_pub_.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; +} + +void Tester::stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg) +{ + state_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::vector points { + size, size, size, -size, -size, -size, -size, 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 { // 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)); +} + +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(); +} + +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 index 5491e948e3..e11f3b1b3d 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -186,7 +186,6 @@ class Tester : public ::testing::Test // CollisionMonitor Action state rclcpp::Subscription::SharedPtr action_state_sub_; nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; - }; // Tester Tester::Tester() diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 9779a97453..836f7219df 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -17,6 +17,7 @@ add_compile_options(-Wno-error=deprecated) rosidl_generate_interfaces(${PROJECT_NAME} "msg/CollisionMonitorState.msg" + "msg/CollisionDetectorState.msg" "msg/Costmap.msg" "msg/CostmapMetaData.msg" "msg/CostmapFilterInfo.msg" diff --git a/nav2_msgs/msg/CollisionDetectorState.msg b/nav2_msgs/msg/CollisionDetectorState.msg new file mode 100644 index 0000000000..9e51b5f507 --- /dev/null +++ b/nav2_msgs/msg/CollisionDetectorState.msg @@ -0,0 +1,4 @@ +# Name of configured polygons +string[] polygons +# List of detections for each polygon +bool[] detections