diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 8544429466a..c9d67df3a1d 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} ) @@ -104,7 +131,7 @@ endif() ### Ament stuff ### ament_export_include_directories(include) -ament_export_libraries(${library_name}) +ament_export_libraries(${monitor_library_name} ${detector_library_name}) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 54841675e90..767390d6500 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -1,5 +1,7 @@ # Nav2 Collision Monitor +## Collision Monitor + The Collision Monitor is a node providing an additional level of robot safety. It performs several collision avoidance related tasks using incoming data from the sensors, bypassing the costmap and trajectory planners, to monitor for and prevent potential collisions at the emergency-stop level. @@ -12,7 +14,7 @@ The costmaps / trajectory planners will handle most situations, but this is to h ![polygons.png](doc/polygons.png) -## Features +### Features The Collision Monitor uses polygons relative the robot's base frame origin to define "zones". Data that fall into these zones trigger an operation depending on the model being used. @@ -37,7 +39,7 @@ The data may be obtained from different data sources: * PointClouds (`sensor_msgs::msg::PointCloud2` messages) * IR/Sonars (`sensor_msgs::msg::Range` messages) -## Design +### Design The Collision Monitor is designed to operate below Nav2 as an independent safety node. This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate. @@ -45,12 +47,12 @@ This acts as a filter on the `cmd_vel` topic coming out of the Controller Server The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model. ![HDL.png](doc/HLD.png) -## Configuration +### Configuration Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages. -## Metrics +### Metrics Designed to be used in wide variety of robots (incl. moving fast) and have a high level of reliability, Collision Monitor node should operate at fast rates. Typical one frame processing time is ~4-5ms for laser scanner (with 360 points) and ~4-20ms for PointClouds (having 24K points). @@ -65,3 +67,9 @@ The following notes could be made: * Due to sheer speed, circle shapes are preferred for the approach behavior models if you can approximately model your robot as circular. * More points mean lower performance. Pointclouds could be culled or filtered before the Collision Monitor to improve performance. + + +## Collision Detector + +Another node exists in the nav2_collision_monitor package called the Collision Detector. This node works similarly to the collision monitor +except that it does not affect the robot's velocity. It will only inform that data from the configured sources has been detected within the configured polygons via message to the `/collision_detector_state` topic. \ No newline at end of file diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp new file mode 100644 index 00000000000..618df25e622 --- /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 d44ef5f509d..8c268e32b88 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 @@ -46,12 +46,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(); @@ -105,14 +105,12 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @brief Supporting routine obtaining all ROS-parameters * @param cmd_vel_in_topic Output name of cmd_vel_in topic * @param cmd_vel_out_topic Output name of cmd_vel_out topic - * @param frequency Frequency of the loop running process_without_vel * is required. * @return True if all parameters were obtained or false in failure case */ bool getParameters( std::string & cmd_vel_in_topic, - std::string & cmd_vel_out_topic, - double & frequency); + std::string & cmd_vel_out_topic); /** * @brief Supporting routine creating and configuring all polygons * @param base_frame_id Robot base frame ID @@ -126,7 +124,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, @@ -146,11 +144,6 @@ class CollisionMonitor : public nav2_util::LifecycleNode */ void process(const Velocity & cmd_vel_in); - /** - * @brief Timer callback for actions not requiring vel - */ - void process_without_vel(); - /** * @brief Processes the polygon of STOP and SLOWDOWN action type * @param polygon Polygon to process @@ -179,18 +172,6 @@ class CollisionMonitor : public nav2_util::LifecycleNode const Velocity & velocity, Action & robot_action) const; - /** - * @brief Processes Publish action type - * @param polygon Polygon to process - * @param collision_points Array of 2D obstacle points - * @param velocity Desired robot velocity - * @param robot_action Output processed robot action - * @return True if returned action is caused by current polygon, otherwise false - */ - bool processPublish( - const std::shared_ptr polygon, - const std::vector & collision_points) const; - /** * @brief Prints robot action and polygon caused it (if it was) * @param robot_action Robot action to print @@ -222,8 +203,6 @@ class CollisionMonitor : public nav2_util::LifecycleNode rclcpp::Subscription::SharedPtr cmd_vel_in_sub_; /// @brief Output cmd_vel publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr cmd_vel_out_pub_; - rclcpp::TimerBase::SharedPtr timer_; - /// @brief Whether main routine is active bool process_active_; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 77f7dc3eb41..8fdbd62e065 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -22,7 +22,6 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" #include "geometry_msgs/msg/polygon.hpp" -#include "std_msgs/msg/bool.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -143,11 +142,6 @@ class Polygon const std::vector & collision_points, const Velocity & velocity) const; - /** - * @brief Publishes detection message - */ - bool publish_detection(std::vector collision_points) const; - /** * @brief Publishes polygon message into a its own topic */ @@ -210,10 +204,6 @@ class Polygon double min_vel_before_stop_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; - /// @brief Detection topic - std::string detection_topic_; - /// @brief Detection publisher - rclcpp_lifecycle::LifecyclePublisher::SharedPtr detection_pub_; // Global variables /// @brief TF buffer diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp index ef311ed8769..715fee22dd7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp @@ -71,8 +71,7 @@ enum ActionType DO_NOTHING = 0, // No action STOP = 1, // Stop the robot SLOWDOWN = 2, // Slowdown in percentage from current operating speed - APPROACH = 3, // Keep constant time interval before collision - PUBLISH = 4 // Publish to a topic + APPROACH = 3 // Keep constant time interval before collision }; /// @brief Action for robot 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 00000000000..ccd58c2e158 --- /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 00000000000..f7fafabedb5 --- /dev/null +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -0,0 +1,24 @@ +collision_detector: + ros__parameters: + 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 00000000000..c4ef21b7d64 --- /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 00000000000..7827f453855 --- /dev/null +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -0,0 +1,338 @@ +// Copyright (c) 2023 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(); + + nav2_util::declare_parameter_if_not_declared( + node, "polygons", rclcpp::ParameterValue(std::vector())); + 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; + } + + // 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_WARN( + get_logger(), + "[%s]: The action_type of the polygon is different than \"none\" which is " + "not supported in the collision detector; it will be considered as \"none\".", + polygon_name.c_str()); + } + + // Configure last added polygon + if (!polygons_.back()->configure()) { + 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()); + } + + 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 8557715d494..74593a13abb 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -24,8 +24,6 @@ #include "nav2_collision_monitor/kinematics.hpp" -using namespace std::chrono_literals; - namespace nav2_collision_monitor { @@ -57,10 +55,9 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) std::string cmd_vel_in_topic; std::string cmd_vel_out_topic; - double frequency; // Obtaining ROS parameters - if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, frequency)) { + if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic)) { return nav2_util::CallbackReturn::FAILURE; } @@ -70,15 +67,6 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) cmd_vel_out_pub_ = this->create_publisher( cmd_vel_out_topic, 1); - for (std::shared_ptr polygon : polygons_) { - if (polygon->getActionType() == PUBLISH) { - timer_ = this->create_wall_timer( - std::chrono::duration{1.0 / frequency}, - std::bind(&CollisionMonitor::process_without_vel, this)); - break; - } - } - return nav2_util::CallbackReturn::SUCCESS; } @@ -188,8 +176,7 @@ void CollisionMonitor::publishVelocity(const Action & robot_action) bool CollisionMonitor::getParameters( std::string & cmd_vel_in_topic, - std::string & cmd_vel_out_topic, - double & frequency) + std::string & cmd_vel_out_topic) { std::string base_frame_id, odom_frame_id; tf2::Duration transform_tolerance; @@ -204,10 +191,6 @@ bool CollisionMonitor::getParameters( node, "cmd_vel_out_topic", rclcpp::ParameterValue("cmd_vel")); cmd_vel_out_topic = get_parameter("cmd_vel_out_topic").as_string(); - nav2_util::declare_parameter_if_not_declared( - node, "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(); @@ -408,34 +391,6 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) robot_action_prev_ = robot_action; } -void CollisionMonitor::process_without_vel() -{ - // Current timestamp for all inner routines prolongation - rclcpp::Time curr_time = this->now(); - - // Do nothing if main worker in non-active state - if (!process_active_) { - return; - } - - // 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); - } - - for (std::shared_ptr polygon : polygons_) { - const ActionType at = polygon->getActionType(); - if (at == PUBLISH) { - // Process PUBLISH for the selected polygon - processPublish(polygon, collision_points); - } - } - publishPolygons(); -} - bool CollisionMonitor::processStopSlowdown( const std::shared_ptr polygon, const std::vector & collision_points, @@ -503,13 +458,6 @@ bool CollisionMonitor::processApproach( return false; } -bool CollisionMonitor::processPublish( - const std::shared_ptr polygon, - const std::vector & collision_points) const -{ - return polygon->publish_detection(collision_points); -} - void CollisionMonitor::printAction( const Action & robot_action, const std::shared_ptr action_polygon) const { @@ -529,10 +477,6 @@ void CollisionMonitor::printAction( get_logger(), "Robot to approach for %f seconds away from collision", action_polygon->getTimeBeforeCollision()); - } else if (robot_action.action_type == PUBLISH) { - RCLCPP_INFO( - get_logger(), - "Robot to publish to configured topic collision detection"); } else { // robot_action.action_type == DO_NOTHING RCLCPP_INFO( get_logger(), diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 9ce615ad25f..86439aa52ed 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -73,11 +73,6 @@ bool Polygon::configure() base_frame_id_, tf2::durationToSec(transform_tolerance_)); } - if (!detection_topic_.empty()) { - detection_pub_ = node->create_publisher( - detection_topic_, rclcpp::SystemDefaultsQoS()); - } - if (visualize_) { // Fill polygon_ points for future usage std::vector poly; @@ -103,9 +98,6 @@ void Polygon::activate() if (visualize_) { polygon_pub_->on_activate(); } - if (!detection_topic_.empty()) { - detection_pub_->on_activate(); - } } void Polygon::deactivate() @@ -113,9 +105,6 @@ void Polygon::deactivate() if (visualize_) { polygon_pub_->on_deactivate(); } - if (!detection_topic_.empty()) { - detection_pub_->on_deactivate(); - } } std::string Polygon::getName() const @@ -238,27 +227,6 @@ void Polygon::publish() const polygon_pub_->publish(std::move(poly_s)); } -bool Polygon::publish_detection(std::vector collision_points) const -{ - if (detection_topic_.empty()) { - return false; - } - - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - bool detected = getPointsInside(collision_points) > getMaxPoints(); - - std::unique_ptr detection = - std::make_unique(); - detection->data = detected; - // Publish polygon - - detection_pub_->publish(std::move(detection)); - return detected; -} - bool Polygon::getCommonParameters(std::string & polygon_pub_topic) { auto node = node_.lock(); @@ -279,8 +247,8 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) action_type_ = SLOWDOWN; } else if (at_str == "approach") { action_type_ = APPROACH; - } else if (at_str == "publish") { - action_type_ = PUBLISH; + } 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; @@ -359,15 +327,6 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp footprint_topic.clear(); } - if (action_type_ == PUBLISH) { - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".detection_topic", rclcpp::ParameterValue(polygon_name_)); - detection_topic_ = - node->get_parameter(polygon_name_ + ".detection_topic").as_string(); - } else { - detection_topic_.clear(); - } - // Leave it not initialized: the will cause an error if it will not set nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt index 77870826b88..f84e83ff218 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -4,7 +4,7 @@ ament_target_dependencies(kinematics_test ${dependencies} ) target_link_libraries(kinematics_test - ${library_name} + ${monitor_library_name} ) # Data sources test @@ -13,7 +13,7 @@ ament_target_dependencies(sources_test ${dependencies} ) target_link_libraries(sources_test - ${library_name} + ${monitor_library_name} ) # Polygon shapes test @@ -22,7 +22,7 @@ ament_target_dependencies(polygons_test ${dependencies} ) target_link_libraries(polygons_test - ${library_name} + ${monitor_library_name} ) # Collision Monitor node test @@ -31,5 +31,5 @@ ament_target_dependencies(collision_monitor_node_test ${dependencies} ) target_link_libraries(collision_monitor_node_test - ${library_name} + ${monitor_library_name} ) diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 2be81db0a74..032ed92625b 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 00000000000..f6a56c01112 --- /dev/null +++ b/nav2_msgs/msg/CollisionDetectorState.msg @@ -0,0 +1,3 @@ +# Name of triggered polygon +string[] polygons +bool[] detections