Merge branch 'ros-planning:main' into rewrite_param-multirobot-namespace-topic

Author: hyunseok-yang

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v15\
+              - "<< parameters.key >>-v16\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v15\
+              - "<< parameters.key >>-v16\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v15\
+            key: "<< parameters.key >>-v16\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp

@@ -69,6 +69,7 @@ class RosTopicLogger : public BT::StatusChangeLogger
     // before converting to a msg.
     event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp));
     event.node_name = node.name();
+    event.uid = node.UID();
     event.previous_status = toStr(prev_status, false);
     event.current_status = toStr(status, false);
     event_log_.push_back(std::move(event));

nav2_bringup/rviz/nav2_default_view.rviz

@@ -560,7 +560,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: -1.6150002479553223
+      Angle: -1.5708
       Class: rviz_default_plugins/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
@@ -570,11 +570,11 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 127.88431549072266
+      Scale: 160
       Target Frame: <Fixed Frame>
       Value: TopDownOrtho (rviz_default_plugins)
-      X: -0.044467076659202576
-      Y: -0.38726311922073364
+      X: 0
+      Y: 0
     Saved: ~
 Window Geometry:
   Displays:

nav2_bringup/rviz/nav2_namespaced_view.rviz

@@ -342,7 +342,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: -1.5700000524520874
+      Angle: -1.5708
       Class: rviz_default_plugins/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
@@ -352,11 +352,11 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 134.638427734375
+      Scale: 160
       Target Frame: <Fixed Frame>
       Value: TopDownOrtho (rviz_default_plugins)
-      X: -0.032615214586257935
-      Y: -0.0801941454410553
+      X: 0
+      Y: 0
     Saved: ~
 Window Geometry:
   Displays:

nav2_bringup/worlds/world_only.model

@@ -16,7 +16,7 @@
 
     <gui fullscreen='0'>
       <camera name='user_camera'>
-        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
+        <pose frame=''>0 0 10 0 1.570796 0</pose>
         <view_controller>orbit</view_controller>
         <projection_type>perspective</projection_type>
       </camera>

nav2_collision_monitor/CMakeLists.txt

@@ -39,10 +39,12 @@ set(dependencies
   nav2_msgs
 )
 
-set(executable_name collision_monitor)
-set(library_name ${executable_name}_core)
+set(monitor_executable_name collision_monitor)
+set(detector_executable_name collision_detector)
+set(monitor_library_name ${monitor_executable_name}_core)
+set(detector_library_name ${detector_executable_name}_core)
 
-add_library(${library_name} SHARED
+add_library(${monitor_library_name} SHARED
   src/collision_monitor_node.cpp
   src/polygon.cpp
   src/circle.cpp
@@ -52,34 +54,59 @@ add_library(${library_name} SHARED
   src/range.cpp
   src/kinematics.cpp
 )
+add_library(${detector_library_name} SHARED
+  src/collision_detector_node.cpp
+  src/polygon.cpp
+  src/circle.cpp
+  src/source.cpp
+  src/scan.cpp
+  src/pointcloud.cpp
+  src/range.cpp
+  src/kinematics.cpp
+)
 
-add_executable(${executable_name}
-  src/main.cpp
+add_executable(${monitor_executable_name}
+  src/collision_monitor_main.cpp
+)
+add_executable(${detector_executable_name}
+  src/collision_detector_main.cpp
 )
 
-ament_target_dependencies(${library_name}
+ament_target_dependencies(${monitor_library_name}
+  ${dependencies}
+)
+ament_target_dependencies(${detector_library_name}
   ${dependencies}
 )
 
-target_link_libraries(${executable_name}
-  ${library_name}
+target_link_libraries(${monitor_executable_name}
+  ${monitor_library_name}
+)
+target_link_libraries(${detector_executable_name}
+  ${detector_library_name}
+)
+
+ament_target_dependencies(${monitor_executable_name}
+  ${dependencies}
 )
 
-ament_target_dependencies(${executable_name}
+ament_target_dependencies(${detector_executable_name}
   ${dependencies}
 )
 
-rclcpp_components_register_nodes(${library_name} "nav2_collision_monitor::CollisionMonitor")
+rclcpp_components_register_nodes(${monitor_library_name} "nav2_collision_monitor::CollisionMonitor")
+
+rclcpp_components_register_nodes(${detector_library_name} "nav2_collision_monitor::CollisionDetector")
 
 ### Install ###
 
-install(TARGETS ${library_name}
+install(TARGETS ${monitor_library_name} ${detector_library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
-install(TARGETS ${executable_name}
+install(TARGETS ${monitor_executable_name} ${detector_executable_name}
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -106,7 +133,7 @@ endif()
 ### Ament stuff ###
 
 ament_export_include_directories(include)
-ament_export_libraries(${library_name})
+ament_export_libraries(${monitor_library_name} ${detector_library_name})
 ament_export_dependencies(${dependencies})
 
 ament_package()

nav2_collision_monitor/README.md

@@ -1,5 +1,7 @@
 # Nav2 Collision Monitor
 
+## Collision Monitor
+
 The Collision Monitor is a node providing an additional level of robot safety.
 It performs several collision avoidance related tasks using incoming data from the sensors, bypassing the costmap and trajectory planners, to monitor for and prevent potential collisions at the emergency-stop level.
 
@@ -12,7 +14,7 @@ The costmaps / trajectory planners will handle most situations, but this is to h
 
 ![polygons.png](doc/polygons.png)
 
-## Features
+### Features
 
 The Collision Monitor uses polygons relative the robot's base frame origin to define "zones".
 Data that fall into these zones trigger an operation depending on the model being used.
@@ -23,7 +25,6 @@ The following models of safety behaviors are employed by Collision Monitor:
 
 * **Stop model**: Define a zone and a point threshold. If more that `N` obstacle points appear inside this area, stop the robot until the obstacles will disappear.
 * **Slowdown model**: Define a zone around the robot and slow the maximum speed for a `%S` percent, if more than `N` points will appear inside the area.
-* **Limit model**: Define a zone around the robot and clamp the maximum speed below a fixed value, if more than `N` points will appear inside the area.
 * **Approach model**: Using the current robot speed, estimate the time to collision to sensor data. If the time is less than `M` seconds (0.5, 2, 5, etc...), the robot will slow such that it is now at least `M` seconds to collision. The effect here would be to keep the robot always `M` seconds from any collision.
 
 The zones around the robot can take the following shapes:
@@ -38,20 +39,19 @@ The data may be obtained from different data sources:
 * PointClouds (`sensor_msgs::msg::PointCloud2` messages)
 * IR/Sonars (`sensor_msgs::msg::Range` messages)
 
-## Design
+### Design
 
 The Collision Monitor is designed to operate below Nav2 as an independent safety node.
 This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate.
 
 The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model.
 ![HLD.png](doc/HLD.png)
-
-## Configuration
+### Configuration
 
 Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages.
 
 
-## Metrics
+### Metrics
 
 Designed to be used in wide variety of robots (incl. moving fast) and have a high level of reliability, Collision Monitor node should operate at fast rates.
 Typical one frame processing time is ~4-5ms for laser scanner (with 360 points) and ~4-20ms for PointClouds (having 24K points).
@@ -66,3 +66,21 @@ The following notes could be made:
 
  * Due to sheer speed, circle shapes are preferred for the approach behavior models if you can approximately model your robot as circular.
  * More points mean lower performance. Pointclouds could be culled or filtered before the Collision Monitor to improve performance.
+
+
+## Collision Detector
+
+In some cases, the user may want to be informed about the detected obstacles without affecting the robot's velocity and instead take a different action within an external node. For example, the user may want to blink LEDs or sound an alarm when the robot is close to an obstacle. Another use case could be to detect data points in particular regions (e.g extremely close to the sensor) and warn of malfunctioning sensors. For this purpose, the Collision Detector node was introduced.
+
+It works similarly to the Collision Monitor, but does not affect the robot's velocity. It will only inform that data from the configured sources has been detected within the configured polygons via message to the `collision_detector_state` topic.
+
+### Features
+
+Similarly to the Collision Monitor, the Collision Detector uses polygons relative the robot's base frame origin to define "zones".
+However, unlike the Collision Monitor that uses different behavior models, the Collision Detector does not use any of them and therefore the `action_type` should always be set to `none`. If set to anything else, it will implicitly be set to `none` and yield a warning.
+
+The zones around the robot and the data sources are the same as for the Collision Monitor, with the exception of the footprint polygon, which is not supported by the Collision Detector.
+
+### Configuration
+
+Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-detector.html).