launch file ROS2

src/modelec_core/launch/modelec.launch.py

@@ -1,12 +1,8 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, Shutdown, RegisterEventHandler, TimerAction
-from launch.conditions import IfCondition
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown, RegisterEventHandler, TimerAction, LogInfo
 from launch.event_handlers import OnProcessExit
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-from ament_index_python.packages import get_package_share_directory
-import os
 
 
 def generate_launch_description():
@@ -16,7 +12,7 @@ def generate_launch_description():
     with_com_arg = DeclareLaunchArgument('with_com', default_value='true', description='Launch COM nodes?')
     with_strat_arg = DeclareLaunchArgument('with_strat', default_value='true', description='Launch strategy nodes?')
 
-    channel_type =  LaunchConfiguration('channel_type', default='serial')
+    channel_type = LaunchConfiguration('channel_type', default='serial')
     serial_port = LaunchConfiguration('serial_port', default='/dev/rplidar')
     serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200')
     frame_id = LaunchConfiguration('frame_id', default='laser')
@@ -24,49 +20,57 @@ def generate_launch_description():
     angle_compensate = LaunchConfiguration('angle_compensate', default='true')
     scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity')
 
-    # Get launch configs
-    with_gui = LaunchConfiguration('with_gui')
-    with_rplidar = LaunchConfiguration('with_rplidar')
-    with_com = LaunchConfiguration('with_com')
-    with_strat = LaunchConfiguration('with_strat')
+    # ----------------------------
+    # Infinite LIDAR restart logic
+    # ----------------------------
+    def create_lidar_with_restart():
+        """Create a lidar node and its auto-restart handler."""
+        lidar_node = Node(
+            package='rplidar_ros',
+            executable='rplidar_node',
+            name='rplidar_node',
+            parameters=[{
+                'channel_type': channel_type,
+                'serial_port': serial_port,
+                'serial_baudrate': serial_baudrate,
+                'frame_id': frame_id,
+                'inverted': inverted,
+                'angle_compensate': angle_compensate,
+                'scan_mode': scan_mode,
+            }],
+            output='screen'
+        )
+
+        # Register a restart handler that re-creates both node + handler
+        restart_handler = RegisterEventHandler(
+            OnProcessExit(
+                target_action=lidar_node,
+                on_exit=[
+                    LogInfo(msg="[Launch] RPLIDAR crashed — restarting in 2s..."),
+                    TimerAction(
+                        period=2.0,
+                        actions=create_lidar_with_restart()  # recursive: spawns new node + handler
+                    )
+                ]
+            )
+        )
+
+        return [lidar_node, restart_handler]
 
     def launch_rplidar_restart_if_needed(context, *args, **kwargs):
         if context.launch_configurations.get('with_rplidar') == 'true':
-            def create_rplidar_node():
-                return Node(
-                    package='rplidar_ros',
-                    executable='rplidar_node',
-                    name='rplidar_node',
-                    parameters=[{
-                        'channel_type': channel_type,
-                        'serial_port': serial_port,
-                        'serial_baudrate': serial_baudrate,
-                        'frame_id': frame_id,
-                        'inverted': inverted,
-                        'angle_compensate': angle_compensate
-                    }],
-                    output='screen'
+            # Start after 3 seconds
+            return [
+                TimerAction(
+                    period=3.0,
+                    actions=create_lidar_with_restart()
                 )
-
-            rplidar_node = create_rplidar_node()
-
-            restart_handler = RegisterEventHandler(
-                OnProcessExit(
-                    target_action=rplidar_node,
-                    on_exit=[
-                        TimerAction(
-                            period=2.0,
-                            actions=[create_rplidar_node()]
-                        )
-                    ]
-                )
-            )
-
-            return [rplidar_node, restart_handler]
+            ]
         return []
 
-
-    # Function to launch GUI and shutdown handler
+    # ----------------------------
+    # GUI node
+    # ----------------------------
     def launch_gui(context, *args, **kwargs):
         if context.launch_configurations.get('with_gui') == 'true':
             gui = Node(
@@ -83,45 +87,39 @@ def generate_launch_description():
             return [gui, shutdown]
         return []
 
-    # Function to launch COM nodes
+    # ----------------------------
+    # COM nodes
+    # ----------------------------
     def launch_com(context, *args, **kwargs):
         if context.launch_configurations.get('with_com') == 'true':
-            serial_listener = Node(
-                package='modelec_com',
-                executable='serial_listener',
-                name='serial_listener'
-            )
-
-            pcb_odo_interface = Node(
-                package='modelec_com',
-                executable='pcb_odo_interface',
-                name='pcb_odo_interface',
-                parameters=[{
-                    'serial_port': "/dev/USB_ODO",
-                    'baudrate': 115200,
-                    'name': "pcb_odo",
-                }]
-            )
-
-            pcb_action_interface = Node(
-                package='modelec_com',
-                executable='pcb_action_interface',
-                name='pcb_action_interface',
-                parameters=[{
-                    'serial_port': "/dev/USB_ACTION",
-                    'baudrate': 115200,
-                    'name': "pcb_action",
-                }]
-            )
-
             return [
-                serial_listener,
-                pcb_odo_interface,
-                pcb_action_interface,
+                Node(package='modelec_com', executable='serial_listener', name='serial_listener'),
+                Node(
+                    package='modelec_com',
+                    executable='pcb_odo_interface',
+                    name='pcb_odo_interface',
+                    parameters=[{
+                        'serial_port': "/dev/USB_ODO",
+                        'baudrate': 115200,
+                        'name': "pcb_odo",
+                    }]
+                ),
+                Node(
+                    package='modelec_com',
+                    executable='pcb_action_interface',
+                    name='pcb_action_interface',
+                    parameters=[{
+                        'serial_port': "/dev/USB_ACTION",
+                        'baudrate': 115200,
+                        'name': "pcb_action",
+                    }]
+                ),
             ]
         return []
 
-    # Function to launch strategy nodes
+    # ----------------------------
+    # Strategy nodes
+    # ----------------------------
     def launch_strat(context, *args, **kwargs):
         if context.launch_configurations.get('with_strat') == 'true':
             return [
@@ -131,41 +129,17 @@ def generate_launch_description():
             ]
         return []
 
+    # ----------------------------
+    # Final launch description
+    # ----------------------------
     return LaunchDescription([
-
-        DeclareLaunchArgument(
-            'channel_type',
-            default_value=channel_type,
-            description='Specifying channel type of lidar'),
-
-        DeclareLaunchArgument(
-            'serial_port',
-            default_value=serial_port,
-            description='Specifying usb port to connected lidar'),
-
-        DeclareLaunchArgument(
-            'serial_baudrate',
-            default_value=serial_baudrate,
-            description='Specifying usb port baudrate to connected lidar'),
-
-        DeclareLaunchArgument(
-            'frame_id',
-            default_value=frame_id,
-            description='Specifying frame_id of lidar'),
-
-        DeclareLaunchArgument(
-            'inverted',
-            default_value=inverted,
-            description='Specifying whether or not to invert scan data'),
-
-        DeclareLaunchArgument(
-            'angle_compensate',
-            default_value=angle_compensate,
-            description='Specifying whether or not to enable angle_compensate of scan data'),
-        DeclareLaunchArgument(
-            'scan_mode',
-            default_value=scan_mode,
-            description='Specifying scan mode of lidar'),
+        DeclareLaunchArgument('channel_type', default_value=channel_type, description='Specifying channel type of lidar'),
+        DeclareLaunchArgument('serial_port', default_value=serial_port, description='Specifying usb port to connected lidar'),
+        DeclareLaunchArgument('serial_baudrate', default_value=serial_baudrate, description='Specifying usb port baudrate to connected lidar'),
+        DeclareLaunchArgument('frame_id', default_value=frame_id, description='Specifying frame_id of lidar'),
+        DeclareLaunchArgument('inverted', default_value=inverted, description='Specifying whether or not to invert scan data'),
+        DeclareLaunchArgument('angle_compensate', default_value=angle_compensate, description='Specifying whether or not to enable angle_compensate of scan data'),
+        DeclareLaunchArgument('scan_mode', default_value=scan_mode, description='Specifying scan mode of lidar'),
 
         with_gui_arg,
         with_rplidar_arg,