Fix #2

jkk-research · Jan 23, 2023 · 603e9e1 · 603e9e1
1 parent f56bda2
commit 603e9e1
Show file tree

Hide file tree

Showing 4 changed files with 55 additions and 129 deletions.
diff --git a/launch/drivers/os_32_right_a.launch.py b/launch/drivers/os_32_right_a.launch.py
@@ -15,84 +15,47 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch_ros.actions import LifecycleNode
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
-from launch.actions import EmitEvent
-from launch.actions import RegisterEventHandler
-from launch_ros.events.lifecycle import ChangeState
-from launch_ros.events.lifecycle import matches_node_name
-from launch_ros.event_handlers import OnStateTransition
-from launch.actions import LogInfo
-from launch.events import matches_action
-from launch.event_handlers.on_shutdown import OnShutdown
+from launch.actions import GroupAction, IncludeLaunchDescription
+from launch_ros.actions import SetRemap
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
-import lifecycle_msgs.msg
 import os
 
-
 def generate_launch_description():
     share_dir = get_package_share_directory('lexus_bringup')
+    ouster_dir = get_package_share_directory('ros2_ouster')
     parameter_file = LaunchConfiguration('params_file')
-    node_name = 'ouster_driver'
     node_id = 'os_right'
-
-    # Acquire the driver param file
+
     params_declare = DeclareLaunchArgument('params_file',
-                                           default_value=os.path.join(
-                                               share_dir, 'launch', 'drivers', 'os_32_right_a_driver_config.yaml'),
-                                           description='FPath to the ROS2 parameters file to use.')
+                                            default_value=os.path.join(
+                                            share_dir, 'launch', 'drivers', 'os_32_right_a_driver_config.yaml'),
+                                            description='FPath to the ROS2 parameters file to use.')
 
-    driver_node = LifecycleNode(package='ros2_ouster',
-                                executable='ouster_driver',
-                                name=node_name,
-                                output='screen',
-                                emulate_tty=True,
-                                parameters=[parameter_file],
-                                arguments=['--ros-args', '--log-level', 'INFO'],
-                                namespace='/',
-                                remappings=[('/points', node_id + '/points')],
-                                )
 
-    configure_event = EmitEvent(
-        event=ChangeState(
-            lifecycle_node_matcher=matches_action(driver_node),
-            transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
-        )
-    )
+    # launch os
+    os_include = GroupAction(
+        actions=[
 
-    activate_event = RegisterEventHandler(
-        OnStateTransition(
-            target_lifecycle_node=driver_node, goal_state='inactive',
-            entities=[
-                LogInfo(
-                    msg="[LifecycleLaunch] Ouster driver node is activating."),
-                EmitEvent(event=ChangeState(
-                    lifecycle_node_matcher=matches_action(driver_node),
-                    transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE,
-                )),
-            ],
-        )
-    )
+            SetRemap(src='/points',dst=node_id + '/points'),
 
-    # TODO make lifecycle transition to shutdown before SIGINT
-    shutdown_event = RegisterEventHandler(
-        OnShutdown(
-            on_shutdown=[
-                EmitEvent(event=ChangeState(
-                  lifecycle_node_matcher=matches_node_name(node_name=node_name),
-                  transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN,
-                )),
-                LogInfo(
-                    msg="[LifecycleLaunch] Ouster driver node is exiting."),
-            ],
-        )
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(ouster_dir + '/launch/driver_launch.py'),
+                launch_arguments = {   
+                    'parameters' : [parameter_file],
+
+
+                }.items(),
+
+            )
+        ]
     )
 
     return LaunchDescription([
         params_declare,
-        driver_node,
-        activate_event,
-        configure_event,
-        shutdown_event,
+        os_include,
     ])
+
+
diff --git a/launch/drivers/os_64_center_a.launch.py b/launch/drivers/os_64_center_a.launch.py
@@ -15,84 +15,47 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch_ros.actions import LifecycleNode
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
-from launch.actions import EmitEvent
-from launch.actions import RegisterEventHandler
-from launch_ros.events.lifecycle import ChangeState
-from launch_ros.events.lifecycle import matches_node_name
-from launch_ros.event_handlers import OnStateTransition
-from launch.actions import LogInfo
-from launch.events import matches_action
-from launch.event_handlers.on_shutdown import OnShutdown
+from launch.actions import GroupAction, IncludeLaunchDescription
+from launch_ros.actions import SetRemap
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
-import lifecycle_msgs.msg
 import os
 
-
 def generate_launch_description():
     share_dir = get_package_share_directory('lexus_bringup')
+    ouster_dir = get_package_share_directory('ros2_ouster')
     parameter_file = LaunchConfiguration('params_file')
-    node_name = 'ouster_driver'
     node_id = 'os_center'
-
-    # Acquire the driver param file
+
     params_declare = DeclareLaunchArgument('params_file',
-                                           default_value=os.path.join(
-                                               share_dir, 'launch', 'drivers', 'os_64_center_a_driver_config.yaml'),
-                                           description='FPath to the ROS2 parameters file to use.')
+                                            default_value=os.path.join(
+                                            share_dir, 'launch', 'drivers', 'os_64_center_a_driver_config.yaml'),
+                                            description='FPath to the ROS2 parameters file to use.')
 
-    driver_node = LifecycleNode(package='ros2_ouster',
-                                executable='ouster_driver',
-                                name=node_name, ## TODO
-                                output='screen',
-                                emulate_tty=True,
-                                parameters=[parameter_file],
-                                arguments=['--ros-args', '--log-level', 'INFO'],
-                                namespace='/',
-                                remappings=[('/points', node_id + '/points')],
-                                )
 
-    configure_event = EmitEvent(
-        event=ChangeState(
-            lifecycle_node_matcher=matches_action(driver_node),
-            transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
-        )
-    )
+    # launch os
+    os_include = GroupAction(
+        actions=[
 
-    activate_event = RegisterEventHandler(
-        OnStateTransition(
-            target_lifecycle_node=driver_node, goal_state='inactive',
-            entities=[
-                LogInfo(
-                    msg="[LifecycleLaunch] Ouster driver node is activating."),
-                EmitEvent(event=ChangeState(
-                    lifecycle_node_matcher=matches_action(driver_node),
-                    transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE,
-                )),
-            ],
-        )
-    )
+            SetRemap(src='/points',dst=node_id + '/points'),
 
-    # TODO make lifecycle transition to shutdown before SIGINT
-    shutdown_event = RegisterEventHandler(
-        OnShutdown(
-            on_shutdown=[
-                EmitEvent(event=ChangeState(
-                  lifecycle_node_matcher=matches_node_name(node_name=node_name),
-                  transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN,
-                )),
-                LogInfo(
-                    msg="[LifecycleLaunch] Ouster driver node is exiting."),
-            ],
-        )
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(ouster_dir + '/launch/driver_launch.py'),
+                launch_arguments = {   
+                    'parameters' : [parameter_file],
+
+
+                }.items(),
+
+            )
+        ]
     )
 
     return LaunchDescription([
         params_declare,
-        driver_node,
-        activate_event,
-        configure_event,
-        shutdown_event,
+        os_include,
     ])
+
+
diff --git a/launch/drivers/os_64_center_a_driver_config.yaml b/launch/drivers/os_64_center_a_driver_config.yaml
@@ -4,8 +4,8 @@ ouster_driver:
     # http://os-992219000350.local/
     computer_ip: 192.168.1.105
     lidar_mode: "1024x10"
-    imu_port: 7503
-    lidar_port: 7502
+    imu_port: 7504
+    lidar_port: 7505
     sensor_frame: os_center_a
     laser_frame: os_center_a_laser_data_frame
     imu_frame: os_center_a_imu_data_frame

diff --git a/launch/tf_static.launch.py b/launch/tf_static.launch.py
@@ -65,22 +65,22 @@ def generate_launch_description():
             executable='static_transform_publisher',
             name='left1_os_front_tf_publisher',
             output='screen',
-            arguments=['1.6', '0.5', '1.3','0', '0', '0', '1','base_link','os_left_a'],
+            arguments=['1.6', '0.5', '1.3','3.14159', '0', '0', 'base_link','os_left_a'],
         ),
         Node(
             package='tf2_ros',
             #namespace='lexus3',
             executable='static_transform_publisher',
             name='right1_os_front_tf_publisher',
             output='screen',
-            arguments=['1.53', '-0.5', '1.41','0', '0', '0', '1','base_link','os_right_a'],
+            arguments=['1.53', '-0.5', '1.41','3.14159', '0', '0','base_link','os_right_a'],
         ),
         Node(
             package='tf2_ros',
             #namespace='lexus3',
             executable='static_transform_publisher',
             name='center1_os_front_tf_publisher',
             output='screen',
-            arguments=['0.75', '0.0', '1.91','0', '0', '0', '1','base_link','os_center_a'],
+            arguments=['0.75', '0.0', '1.91','3.14159', '0', '0','base_link','os_center_a'],
         )
     ])