generic launch for pipeline

mrpt_pointcloud_pipeline/launch/demo_with_mvsim.launch.py → mrpt_pointcloud_pipeline/launch/pointcloud_pipeline.launch.py

@@ -1,21 +1,32 @@
-import os
+# ROS 2 launch file for mrpt_pointcloud_pipeline, intended to be included in user's
+# launch files.
+#
+# See the docs on the configurable launch arguments for this file in:
+# https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_pointcloud_pipeline
+#
+# For ready-to-launch demos, see: https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_tutorials
+#
+
 from launch import LaunchDescription
-from launch_ros.actions import Node
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import TextSubstitution
 from launch.substitutions import LaunchConfiguration
-from ament_index_python.packages import get_package_share_directory
-from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument, Shutdown
+from ament_index_python import get_package_share_directory
+import os
 
 
 def generate_launch_description():
+    myPkgDir = get_package_share_directory("mrpt_pointcloud_pipeline")
+    # print('myPkgDir       : ' + myPkgDir)
 
     lidar_topic_name_arg = DeclareLaunchArgument(
-        'lidar_topic_name',
-        default_value='/laser1, /laser2'
+        'scan_topic_name',
+        default_value=''  # /scan, /laser1, etc.
     )
     points_topic_name_arg = DeclareLaunchArgument(
         'points_topic_name',
-        default_value='/ouster/points, /camera1_points'
+        default_value=''  # '/ouster/points', etc.
     )
     show_gui_arg = DeclareLaunchArgument(
         'show_gui',
@@ -27,8 +38,8 @@ def generate_launch_description():
     )
     filter_yaml_file_arg = DeclareLaunchArgument(
         'filter_yaml_file',
-        default_value=os.path.join(os.path.dirname(
-            __file__), 'local-obstacles-decimation-filter.yaml')
+        default_value=os.path.join(
+            myPkgDir, 'params', 'local-obstacles-decimation-filter.yaml')
     )
     filter_output_layer_name_arg = DeclareLaunchArgument(
         'filter_output_layer_name',
@@ -47,14 +58,21 @@ def generate_launch_description():
         default_value='base_link'
     )
 
-    # Node: Local obstacles builder
+    log_level_launch_arg = DeclareLaunchArgument(
+        "log_level",
+        default_value=TextSubstitution(text=str("INFO")),
+        description="Logging level"
+    )
+
+    emit_shutdown_action = Shutdown(reason='launch is shutting down')
+
     mrpt_pointcloud_pipeline_node = Node(
         package='mrpt_pointcloud_pipeline',
         executable='mrpt_pointcloud_pipeline_node',
         name='mrpt_pointcloud_pipeline_node',
         output='screen',
         parameters=[
-            {'source_topics_2dscan': LaunchConfiguration('lidar_topic_name')},
+            {'source_topics_2d_scans': LaunchConfiguration('scan_topic_name')},
             {'source_topics_pointclouds': LaunchConfiguration(
                 'points_topic_name')},
             {'show_gui': LaunchConfiguration('show_gui')},
@@ -64,26 +82,16 @@ def generate_launch_description():
             {'time_window': LaunchConfiguration('time_window')},
             {'topic_local_map_pointcloud': LaunchConfiguration(
                 'filter_output_topic_name')},
-            {'frameid_reference': LaunchConfiguration('frameid_reference')},
+            {'frameid_reference': LaunchConfiguration(
+                'frameid_reference')},
             {'frameid_robot': LaunchConfiguration('frameid_robot')},
         ],
+        arguments=['--ros-args', '--log-level',
+                   LaunchConfiguration('log_level')],
+        on_exit=[emit_shutdown_action]
     )
 
-    mvsim_pkg_share_dir = get_package_share_directory('mvsim')
-    # Finding the launch file
-    launch_file_name = 'demo_jackal.launch.py'
-    mvsim_launch_file_path = os.path.join(
-        mvsim_pkg_share_dir, 'mvsim_tutorial', launch_file_name)
-
-    # Check if the launch file exists
-    if not os.path.isfile(mvsim_launch_file_path):
-        raise Exception(
-            f"Launch file '{mvsim_launch_file_path}' does not exist!")
-
     return LaunchDescription([
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(mvsim_launch_file_path)
-        ),
         lidar_topic_name_arg,
         points_topic_name_arg,
         show_gui_arg,
@@ -93,5 +101,6 @@ def generate_launch_description():
         filter_output_topic_arg,
         frameid_reference_arg,
         frameid_robot_arg,
-        mrpt_pointcloud_pipeline_node
+        log_level_launch_arg,
+        mrpt_pointcloud_pipeline_node,
     ])

mrpt_pointcloud_pipeline/src/mrpt_pointcloud_pipeline_node.cpp

@@ -52,11 +52,15 @@ LocalObstaclesNode::LocalObstaclesNode(const rclcpp::NodeOptions& options)
 		static_cast<unsigned int>(nSubsTotal));
 
 	if (!(nSubsTotal > 0))
+	{
 		RCLCPP_ERROR(
 			get_logger(),
 			"*Error* It is mandatory to set at least one source topic for "
 			"sensory information!");
 
+		rclcpp::shutdown();
+	}
+
 	// Local map params:
 	m_localmap_pts->insertionOptions.minDistBetweenLaserPoints = 0;
 	m_localmap_pts->insertionOptions.also_interpolate = false;
@@ -513,10 +517,10 @@ void LocalObstaclesNode::read_parameters()
 		m_topic_local_map_pointcloud.c_str());
 
 	this->declare_parameter<std::string>(
-		"source_topics_2dscan", "scan, laser1");
-	this->get_parameter("source_topics_2dscan", m_topics_source_2dscan);
+		"source_topics_2d_scans", "scan, laser1");
+	this->get_parameter("source_topics_2d_scans", m_topics_source_2dscan);
 	RCLCPP_INFO(
-		get_logger(), "source_topics_2dscan: %s",
+		get_logger(), "source_topics_2d_scans: %s",
 		m_topics_source_2dscan.c_str());
 
 	this->declare_parameter<std::string>("source_topics_pointclouds", "");

mrpt_tutorials/CMakeLists.txt

@@ -7,7 +7,6 @@ find_package(ament_cmake REQUIRED)
 # Mark other files for installation (e.g. launch and bag files, etc.)
 install(DIRECTORY
   datasets
-  gazebo
   launch
   maps
   mvsim

mrpt_tutorials/launch/demo_pointcloud_pipeline_mvsim.launch.py

@@ -0,0 +1,47 @@
+# ROS 2 launch file for example in mrpt_tutorials
+#
+# See repo online: https://github.com/mrpt-ros-pkg/mrpt_navigation
+#
+
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+
+    # Launch for mrpt_pointcloud_pipeline:
+    pointcloud_pipeline_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('mrpt_pointcloud_pipeline'), 'launch',
+            'pointcloud_pipeline.launch.py')]),
+        launch_arguments={
+            'log_level': 'INFO',
+            'scan_topic_name': '/laser1, /laser2',
+            'points_topic_name': '/camera1_points',
+            'time_window': '0.20',
+            'show_gui': 'True',
+        }.items()
+    )
+
+    mvsim_pkg_share_dir = get_package_share_directory('mvsim')
+    # Finding the launch file
+    launch_file_name = 'demo_jackal.launch.py'
+    mvsim_launch_file_path = os.path.join(
+        mvsim_pkg_share_dir, 'mvsim_tutorial', launch_file_name)
+
+    # Check if the launch file exists
+    if not os.path.isfile(mvsim_launch_file_path):
+        raise Exception(
+            f"Launch file '{mvsim_launch_file_path}' does not exist!")
+
+    return LaunchDescription([
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(mvsim_launch_file_path)
+        ),
+        pointcloud_pipeline_launch
+    ])

mrpt_tutorials/launch/demo_reactive_nav_mvsim.launch.py

@@ -1,6 +1,9 @@
-import os
-import sys
+# ROS 2 launch file for example in mrpt_tutorials
+#
+# See repo online: https://github.com/mrpt-ros-pkg/mrpt_navigation
+#
 
+import os
 from launch import LaunchDescription
 from ament_index_python.packages import get_package_share_directory
 from launch_ros.actions import Node
@@ -42,7 +45,7 @@ def generate_launch_description():
         parameters=[
             {
                 # 2D lidar sources:
-                'source_topics_2dscan': 'scanner1',
+                'source_topics_2d_scans': 'scanner1',
                 # 3D lidar sources:
                 'source_topics_pointclouds': 'lidar1_points',
                 'filter_yaml_file': LaunchConfiguration('filter_yaml_file'),