diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 8544429466..546d080790 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -37,10 +37,12 @@ set(dependencies nav2_costmap_2d ) -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 @@ -50,34 +52,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} ) @@ -98,13 +125,12 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(test) 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/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 2aed9920d0..da94591117 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(); @@ -125,7 +125,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..2d6ff099a3 --- /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_wall_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->getMaxPoints() + 1); + } + + 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 69523e3c14..a6e3f86806 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -242,8 +242,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 diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index d17c34b46a..99856eb0b0 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -234,6 +234,8 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) action_type_ = SLOWDOWN; } 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; diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt deleted file mode 100644 index 77870826b8..0000000000 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -# Kinematics test -ament_add_gtest(kinematics_test kinematics_test.cpp) -ament_target_dependencies(kinematics_test - ${dependencies} -) -target_link_libraries(kinematics_test - ${library_name} -) - -# Data sources test -ament_add_gtest(sources_test sources_test.cpp) -ament_target_dependencies(sources_test - ${dependencies} -) -target_link_libraries(sources_test - ${library_name} -) - -# Polygon shapes test -ament_add_gtest(polygons_test polygons_test.cpp) -ament_target_dependencies(polygons_test - ${dependencies} -) -target_link_libraries(polygons_test - ${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 - ${library_name} -) diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp deleted file mode 100644 index 472530f786..0000000000 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ /dev/null @@ -1,933 +0,0 @@ -// 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 "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_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 FOOTPRINT_TOPIC[]{"footprint"}; -static const char SCAN_NAME[]{"Scan"}; -static const char POINTCLOUD_NAME[]{"PointCloud"}; -static const char RANGE_NAME[]{"Range"}; -static const int MAX_POINTS{1}; -static const double SLOWDOWN_RATIO{0.7}; -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 -}; - -enum SourceType -{ - SOURCE_UNKNOWN = 0, - SCAN = 1, - POINTCLOUD = 2, - RANGE = 3 -}; - -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); - 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 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); - -protected: - void cmdVelOutCallback(geometry_msgs::msg::Twist::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_; - - // 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_; -}; // 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()); - - 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)); -} - -Tester::~Tester() -{ - footprint_pub_.reset(); - - scan_pub_.reset(); - pointcloud_pub_.reset(); - range_pub_.reset(); - - cmd_vel_in_pub_.reset(); - cmd_vel_out_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( - "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) -{ - 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::vector points { - size, size, size, -size, -size, -size, -size, 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 { // 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 + ".action_type", rclcpp::ParameterValue(at)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + ".action_type", at)); - - cm_->declare_parameter( - polygon_name + ".max_points", rclcpp::ParameterValue(MAX_POINTS)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + ".max_points", MAX_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 + ".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)); -} - -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 { // 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::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::publishCmdVel(const double x, const double y, const double tw) -{ - // Reset cmd_vel_out_ before calling CollisionMonitor::process() - cmd_vel_out_ = 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; -} - -void Tester::cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg) -{ - cmd_vel_out_ = msg; -} - -TEST_F(Tester, testProcessStopSlowdown) -{ - rclcpp::Time curr_time = cm_->now(); - - // Set Collision Monitor parameters. - // Making two polygons: outer polygon for slowdown and inner for robot stop. - setCommonParameters(); - addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); - addPolygon("Stop", POLYGON, 1.0, "stop"); - addSource(SCAN_NAME, SCAN); - setVectors({"SlowDown", "Stop"}, {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.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 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); - - // 3. 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); - - // 4. Restoring back 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); - - // 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); - - // 3. Obstacle is inside robot footprint - publishScan(0.5, curr_time); - ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); - publishCmdVel(0.5, 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); - - // 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)); - - // 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); - - // 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); - 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); - - // 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); - - // 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); - - // 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); - - // 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); - - // 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); - - // 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, 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({"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(); -} - -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 deleted file mode 100644 index 65933daa38..0000000000 --- a/nav2_collision_monitor/test/kinematics_test.cpp +++ /dev/null @@ -1,98 +0,0 @@ -// 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 deleted file mode 100644 index 87576ddda9..0000000000 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ /dev/null @@ -1,698 +0,0 @@ -// 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 FOOTPRINT_TOPIC[]{"footprint"}; -static const char POLYGON_PUB_TOPIC[]{"polygon"}; -static const char POLYGON_NAME[]{"TestPolygon"}; -static const char CIRCLE_NAME[]{"TestCircle"}; -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 double CIRCLE_RADIUS{0.5}; -static const int MAX_POINTS{1}; -static const double SLOWDOWN_RATIO{0.7}; -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() - { - footprint_pub_.reset(); - } - - 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 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); - void setPolygonParameters(const std::vector & points); - void setCircleParameters(const double radius); - bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param); - // Creating routines - void createPolygon(const std::string & action_type); - void createCircle(const std::string & action_type); - - // 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) -{ - 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 + ".max_points", rclcpp::ParameterValue(MAX_POINTS)); - test_node_->set_parameter( - rclcpp::Parameter(polygon_name + ".max_points", MAX_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 + ".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)); -} - -void Tester::setPolygonParameters(const std::vector & points) -{ - 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)); - - test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(points)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", points)); -} - -void Tester::setCircleParameters(const double radius) -{ - test_node_->declare_parameter( - std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius)); -} - -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) -{ - setCommonParameters(POLYGON_NAME, action_type); - setPolygonParameters(SQUARE_POLYGON); - - 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) -{ - setCommonParameters(CIRCLE_NAME, action_type); - setCircleParameters(CIRCLE_RADIUS); - - circle_ = std::make_shared( - test_node_, CIRCLE_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); - ASSERT_TRUE(circle_->configure()); - circle_->activate(); -} - -bool Tester::waitFootprint( - const std::chrono::nanoseconds & timeout, - std::vector & footprint) -{ - rclcpp::Time start_time = test_node_->now(); - while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { - polygon_->updatePolygon(); - 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"); - - // Check that common parameters set correctly - EXPECT_EQ(polygon_->getName(), POLYGON_NAME); - EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::STOP); - EXPECT_EQ(polygon_->getMaxPoints(), MAX_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"); - - // Check that common parameters set correctly - EXPECT_EQ(polygon_->getName(), POLYGON_NAME); - EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::SLOWDOWN); - EXPECT_EQ(polygon_->getMaxPoints(), MAX_POINTS); - EXPECT_EQ(polygon_->isVisualize(), true); - // Check that slowdown_ratio is correct - EXPECT_NEAR(polygon_->getSlowdownRatio(), SLOWDOWN_RATIO, EPSILON); -} - -TEST_F(Tester, testPolygonGetApproachParameters) -{ - createPolygon("approach"); - - // Check that common parameters set correctly - EXPECT_EQ(polygon_->getName(), POLYGON_NAME); - EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::APPROACH); - EXPECT_EQ(polygon_->getMaxPoints(), MAX_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"); - - // Check that common parameters set correctly - EXPECT_EQ(circle_->getName(), CIRCLE_NAME); - EXPECT_EQ(circle_->getActionType(), nav2_collision_monitor::APPROACH); - EXPECT_EQ(circle_->getMaxPoints(), MAX_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" parameter is 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" parameter is not set after configuring - ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "points")); -} - -TEST_F(Tester, testPolygonIncorrectActionType) -{ - setCommonParameters(POLYGON_NAME, "incorrect_action_type"); - setPolygonParameters(SQUARE_POLYGON); - - 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"); - - std::vector incorrect_points = SQUARE_POLYGON; - incorrect_points.resize(6); // Not enough for triangle - test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points)); - - 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"); - - std::vector incorrect_points = SQUARE_POLYGON; - incorrect_points.resize(9); // Odd number of points - test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points)); - - polygon_ = std::make_shared( - test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); - ASSERT_FALSE(polygon_->configure()); -} - -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" parameter is not set after configuring - ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "radius")); -} - -TEST_F(Tester, testPolygonUpdate) -{ - createPolygon("approach"); - - std::vector poly; - polygon_->getPolygon(poly); - ASSERT_EQ(poly.size(), 0u); - - test_node_->publishFootprint(); - - std::vector footprint; - ASSERT_TRUE(waitFootprint(500ms, footprint)); - - 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"); - - 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); - - 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"); - - 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"); - - // 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::vector points{{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, 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.clear(); - points = {{-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, 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.clear(); - points = {{-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, 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.clear(); - points = {{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, 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.clear(); - points = {{0.1, -0.01}, {0.1, 0.01}}; - // Collision already appeared: collision time should be 0 - EXPECT_NEAR(polygon_->getCollisionTime(points, 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.clear(); - points = {{1.1, -0.01}, {1.1, 0.01}}; - // There is no collision: return value should be negative - EXPECT_LT(polygon_->getCollisionTime(points, vel), 0.0); -} - -TEST_F(Tester, testPolygonPublish) -{ - createPolygon("stop"); - 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")); - setPolygonParameters(SQUARE_POLYGON); - - // 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); -} - -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 deleted file mode 100644 index 0bfcd1a062..0000000000 --- a/nav2_collision_monitor/test/sources_test.cpp +++ /dev/null @@ -1,635 +0,0 @@ -// 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" - -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 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(); - } - - 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)); - } - -private: - rclcpp::Publisher::SharedPtr scan_pub_; - rclcpp::Publisher::SharedPtr pointcloud_pub_; - rclcpp::Publisher::SharedPtr range_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 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); - void checkScan(const std::vector & data); - void checkPointCloud(const std::vector & data); - void checkRange(const std::vector & data); - - std::shared_ptr test_node_; - std::shared_ptr scan_; - std::shared_ptr pointcloud_; - std::shared_ptr range_; - -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(); -} - -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; -} - -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); -} - -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); - - // Wait until all sources will receive the data - ASSERT_TRUE(waitScan(500ms)); - ASSERT_TRUE(waitPointCloud(500ms)); - ASSERT_TRUE(waitRange(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); -} - -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); - - // Wait until all sources will receive the data - ASSERT_TRUE(waitScan(500ms)); - ASSERT_TRUE(waitPointCloud(500ms)); - ASSERT_TRUE(waitRange(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); -} - -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); - - // Wait until all sources will receive the data - ASSERT_TRUE(waitScan(500ms)); - ASSERT_TRUE(waitPointCloud(500ms)); - ASSERT_TRUE(waitRange(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); -} - -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); - - // Wait until all sources will receive the data - ASSERT_TRUE(waitScan(500ms)); - ASSERT_TRUE(waitPointCloud(500ms)); - ASSERT_TRUE(waitRange(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); -} - -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 82c618689d..8cb89e43e8 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(action_msgs REQUIRED) nav2_package() rosidl_generate_interfaces(${PROJECT_NAME} + "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