mujoco_ros2_control/mujoco_ros2_control_demos/launch/camera_example.launch.py at 90cf2c90ed03d0cc58bb9d8d8470a3f33d5655c2 · moveit/mujoco_ros2_control · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
import os
import xacro

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import ExecuteProcess, RegisterEventHandler
from launch.event_handlers import OnProcessStart, OnProcessExit

from launch_ros.actions import Node


def generate_launch_description():

    mujoco_ros2_control_demos_path = os.path.join(
        get_package_share_directory('mujoco_ros2_control_demos'))
    xacro_file = os.path.join(mujoco_ros2_control_demos_path,
                              'urdf',
                              'test_camera.xacro.urdf')
    controller_config_file = os.path.join(mujoco_ros2_control_demos_path,
                                          'config',
                                          'camera_controller_position.yaml')
    rviz_config_file = os.path.join(mujoco_ros2_control_demos_path,
                                          'launch',
                                          'camera_demo.rviz')

    doc = xacro.parse(open(xacro_file))
    xacro.process_doc(doc)
    robot_description = {'robot_description': doc.toxml()}

    use_sim_time = {'use_sim_time': True}

    node_mujoco_ros2_control = Node(
        package='mujoco_ros2_control',
        executable='mujoco_ros2_control',
        output='screen',
        parameters=[
            robot_description,
            controller_config_file,
            use_sim_time,
            {'mujoco_model_path' : os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_camera.xml')}
        ]
    )

    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[
            use_sim_time,
            robot_description,
        ]
    )

    load_joint_state_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', '--use-sim-time',
             'joint_state_broadcaster'],
        output='screen'
    )

    load_position_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', '--use-sim-time',
             'position_controller'],
        output='screen'
    )

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        parameters=[use_sim_time],
        arguments=["-d", rviz_config_file],
    )

    return LaunchDescription([
        node_mujoco_ros2_control,
        node_robot_state_publisher,
        RegisterEventHandler(
            event_handler=OnProcessStart(
                target_action=node_robot_state_publisher,
                on_start=[load_joint_state_controller],
            )
        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=load_joint_state_controller,
                on_exit=[load_position_controller],
            )
        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=load_position_controller,
                on_exit=[rviz_node],
            )
        ),
    ])