Merge pull request #29 from botsandus/AUTO-1589_CM_smoother_sync

AUTO-1589 Minimal nav2 update

nav2_collision_monitor/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(nav2_common REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 
 ### Header ###
 
@@ -37,6 +38,7 @@ set(dependencies
   nav2_util
   nav2_costmap_2d
   nav2_msgs
+  visualization_msgs
 )
 
 set(monitor_executable_name collision_monitor)

nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp

@@ -28,6 +28,7 @@
 
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/msg/collision_detector_state.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/polygon.hpp"
@@ -147,6 +148,9 @@ class CollisionDetector : public nav2_util::LifecycleNode
   /// @brief collision monitor state publisher
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionDetectorState>::SharedPtr
     state_pub_;
+  /// @brief Collision points marker publisher
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    collision_points_marker_pub_;
   /// @brief timer that runs actions
   rclcpp::TimerBase::SharedPtr timer_;
 

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -21,6 +21,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
@@ -107,6 +108,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    * @param cmd_vel_in_topic Output name of cmd_vel_in topic
    * @param cmd_vel_out_topic Output name of cmd_vel_out topic
    * is required.
+   * @param state_topic topic name for publishing collision monitor state
    * @return True if all parameters were obtained or false in failure case
    */
   bool getParameters(
@@ -210,6 +212,10 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionMonitorState>::SharedPtr
     state_pub_;
 
+  /// @brief Collision points marker publisher
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    collision_points_marker_pub_;
+
   /// @brief Whether main routine is active
   bool process_active_;
 

nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp

@@ -69,8 +69,9 @@ class PointCloud : public Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  void getData(
+  bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const;
 

nav2_collision_monitor/include/nav2_collision_monitor/range.hpp

@@ -69,8 +69,9 @@ class Range : public Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  void getData(
+  bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const;
 

nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp

@@ -69,8 +69,9 @@ class Scan : public Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  void getData(
+  bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const;
 

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp

@@ -69,8 +69,9 @@ class Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  virtual void getData(
+  virtual bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const = 0;
 
@@ -80,6 +81,18 @@ class Source
    */
   bool getEnabled() const;
 
+  /**
+   * @brief Obtains the name of the data source
+   * @return Name of the data source
+   */
+  std::string getSourceName() const;
+
+  /**
+   * @brief Obtains the source_timeout parameter of the data source
+   * @return source_timeout parameter value of the data source
+   */
+  rclcpp::Duration getSourceTimeout() const;
+
 protected:
   /**
    * @brief Source configuration routine.

nav2_collision_monitor/launch/collision_detector_node.launch.py

@@ -37,8 +37,7 @@ def generate_launch_description():
     # Constant parameters
     lifecycle_nodes = ['collision_detector']
     autostart = True
-    remappings = [('/tf', 'tf'),
-                  ('/tf_static', 'tf_static')]
+    remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
 
     # Launch arguments
     # 1. Create the launch configuration variables
@@ -51,87 +50,106 @@ def generate_launch_description():
 
     # 2. Declare the launch arguments
     declare_namespace_cmd = DeclareLaunchArgument(
-        'namespace',
-        default_value='',
-        description='Top-level namespace')
+        'namespace', default_value='', description='Top-level namespace'
+    )
 
     declare_use_sim_time_cmd = DeclareLaunchArgument(
         'use_sim_time',
         default_value='True',
-        description='Use simulation (Gazebo) clock if true')
+        description='Use simulation (Gazebo) clock if true',
+    )
 
     declare_params_file_cmd = DeclareLaunchArgument(
         'params_file',
-        default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'),
-        description='Full path to the ROS2 parameters file to use for all launched nodes')
+        default_value=os.path.join(
+            package_dir, 'params', 'collision_monitor_params.yaml'
+        ),
+        description='Full path to the ROS2 parameters file to use for all launched nodes',
+    )
 
     declare_use_composition_cmd = DeclareLaunchArgument(
-        'use_composition', default_value='True',
-        description='Use composed bringup if True')
+        'use_composition',
+        default_value='True',
+        description='Use composed bringup if True',
+    )
 
     declare_container_name_cmd = DeclareLaunchArgument(
-        'container_name', default_value='nav2_container',
-        description='the name of conatiner that nodes will load in if use composition')
+        'container_name',
+        default_value='nav2_container',
+        description='the name of conatiner that nodes will load in if use composition',
+    )
 
     configured_params = RewrittenYaml(
         source_file=params_file,
         root_key=namespace,
         param_rewrites={},
-        convert_types=True)
+        convert_types=True,
+    )
 
     # Declare node launching commands
     load_nodes = GroupAction(
         condition=IfCondition(PythonExpression(['not ', use_composition])),
         actions=[
             SetParameter('use_sim_time', use_sim_time),
             PushROSNamespace(
-                condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')),
-                namespace=namespace),
+                condition=IfCondition(
+                    NotEqualsSubstitution(LaunchConfiguration('namespace'), '')
+                ),
+                namespace=namespace,
+            ),
             Node(
                 package='nav2_lifecycle_manager',
                 executable='lifecycle_manager',
                 name='lifecycle_manager_collision_detector',
                 output='screen',
                 emulate_tty=True,  # https://github.com/ros2/launch/issues/188
-                parameters=[{'autostart': autostart},
-                            {'node_names': lifecycle_nodes}],
-                remappings=remappings),
+                parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}],
+                remappings=remappings,
+            ),
             Node(
                 package='nav2_collision_monitor',
                 executable='collision_detector',
                 output='screen',
                 emulate_tty=True,  # https://github.com/ros2/launch/issues/188
                 parameters=[configured_params],
-                remappings=remappings)
-        ]
+                remappings=remappings,
+            ),
+        ],
     )
 
     load_composable_nodes = GroupAction(
         condition=IfCondition(use_composition),
         actions=[
             SetParameter('use_sim_time', use_sim_time),
             PushROSNamespace(
-                condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')),
-                namespace=namespace),
+                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),
+                        parameters=[
+                            {'autostart': autostart},
+                            {'node_names': lifecycle_nodes},
+                        ],
+                        remappings=remappings,
+                    ),
                     ComposableNode(
                         package='nav2_collision_monitor',
                         plugin='nav2_collision_monitor::CollisionDetector',
                         name='collision_detector',
                         parameters=[configured_params],
-                        remappings=remappings)
+                        remappings=remappings,
+                    ),
                 ],
-            )
-        ]
+            ),
+        ],
     )
 
     ld = LaunchDescription()