|
1 | 1 | from launch import LaunchDescription |
2 | | -from launch.substitutions import LaunchConfiguration |
3 | | -from lbr_bringup.description import LBRDescriptionMixin |
4 | | -from lbr_bringup.gazebo import GazeboMixin |
5 | | -from lbr_bringup.ros2_control import LBRROS2ControlMixin |
| 2 | +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription |
| 3 | +from launch.substitutions import ( |
| 4 | + Command, |
| 5 | + FindExecutable, |
| 6 | + LaunchConfiguration, |
| 7 | + PathSubstitution, |
| 8 | +) |
| 9 | +from launch_ros.actions import Node |
| 10 | +from launch_ros.substitutions import FindPackageShare |
6 | 11 |
|
7 | 12 |
|
8 | 13 | def generate_launch_description() -> LaunchDescription: |
9 | | - ld = LaunchDescription() |
10 | | - |
11 | | - # launch arguments |
12 | | - ld.add_action(LBRDescriptionMixin.arg_model()) |
13 | | - ld.add_action(LBRDescriptionMixin.arg_robot_name()) |
14 | | - ld.add_action(LBRROS2ControlMixin.arg_init_jnt_pos()) |
15 | | - ld.add_action( |
16 | | - LBRROS2ControlMixin.arg_ctrl() |
17 | | - ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/ros2_control/lbr_controllers.yaml |
18 | | - |
19 | | - # robot description |
20 | | - robot_description = LBRDescriptionMixin.param_robot_description(mode="gazebo") |
21 | | - |
22 | | - # robot state publisher |
23 | | - robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( |
24 | | - robot_description=robot_description, use_sim_time=True |
25 | | - ) |
26 | | - ld.add_action( |
27 | | - robot_state_publisher |
28 | | - ) # Do not condition robot state publisher on joint state broadcaster as Gazebo uses robot state publisher to retrieve robot description |
29 | | - |
30 | | - # Gazebo |
31 | | - ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager |
32 | | - ld.add_action(GazeboMixin.node_clock_bridge()) |
33 | | - ld.add_action( |
34 | | - GazeboMixin.node_create() |
35 | | - ) # spawns robot in Gazebo through robot_description topic of robot_state_publisher |
36 | | - |
37 | | - # spawn controllers |
38 | | - ld.add_action( |
39 | | - LBRROS2ControlMixin.node_controller_spawner( |
40 | | - controllers=["joint_state_broadcaster", LaunchConfiguration("ctrl")] |
41 | | - ) |
| 14 | + return LaunchDescription( |
| 15 | + [ |
| 16 | + DeclareLaunchArgument( |
| 17 | + name="model", |
| 18 | + default_value="iiwa7", |
| 19 | + description="The LBR model in use.", |
| 20 | + choices=["iiwa7", "iiwa14", "med7", "med14"], |
| 21 | + ), |
| 22 | + DeclareLaunchArgument( |
| 23 | + name="robot_name", |
| 24 | + default_value="lbr", |
| 25 | + description="The robot's name.", |
| 26 | + ), |
| 27 | + DeclareLaunchArgument( |
| 28 | + name="init_jnt_pos_pkg", |
| 29 | + default_value="lbr_description", |
| 30 | + description="Package containing the initial_joint_positions.yaml file.", |
| 31 | + ), |
| 32 | + DeclareLaunchArgument( |
| 33 | + name="init_jnt_pos", |
| 34 | + default_value="ros2_control/initial_joint_positions.yaml", |
| 35 | + description="The relative path from sys_cfg_pkg to the initial_joint_positions.yaml file.", |
| 36 | + ), |
| 37 | + DeclareLaunchArgument( |
| 38 | + name="ctrl", |
| 39 | + default_value="joint_trajectory_controller", |
| 40 | + description="Desired default controller. Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/ros2_control/gazebo_controllers.yaml.", |
| 41 | + choices=[ |
| 42 | + "forward_position_controller", |
| 43 | + "joint_trajectory_controller", |
| 44 | + ], |
| 45 | + ), |
| 46 | + Node( |
| 47 | + package="robot_state_publisher", |
| 48 | + executable="robot_state_publisher", |
| 49 | + output="screen", |
| 50 | + parameters=[ |
| 51 | + { |
| 52 | + "robot_description": Command( |
| 53 | + [ |
| 54 | + FindExecutable(name="xacro"), |
| 55 | + " ", |
| 56 | + PathSubstitution(FindPackageShare("lbr_description")) |
| 57 | + / "urdf" |
| 58 | + / LaunchConfiguration("model") |
| 59 | + / LaunchConfiguration("model"), |
| 60 | + ".xacro", |
| 61 | + " robot_name:=", |
| 62 | + LaunchConfiguration("robot_name"), |
| 63 | + " mode:=gazebo", |
| 64 | + " initial_joint_positions_path:=", |
| 65 | + PathSubstitution( |
| 66 | + FindPackageShare( |
| 67 | + LaunchConfiguration("init_jnt_pos_pkg") |
| 68 | + ) |
| 69 | + ) |
| 70 | + / LaunchConfiguration( |
| 71 | + "init_jnt_pos", |
| 72 | + ), |
| 73 | + ] |
| 74 | + ) |
| 75 | + }, |
| 76 | + {"use_sim_time": True}, |
| 77 | + ], |
| 78 | + namespace=LaunchConfiguration("robot_name"), |
| 79 | + ), |
| 80 | + IncludeLaunchDescription( |
| 81 | + PathSubstitution( |
| 82 | + FindPackageShare("ros_gz_sim"), |
| 83 | + ) |
| 84 | + / "launch" |
| 85 | + / "gz_sim.launch.py", |
| 86 | + launch_arguments={"gz_args": "-r empty.sdf"}.items(), |
| 87 | + ), # Gazebo has its own controller manager |
| 88 | + Node( |
| 89 | + package="ros_gz_bridge", |
| 90 | + executable="parameter_bridge", |
| 91 | + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], |
| 92 | + output="screen", |
| 93 | + ), |
| 94 | + Node( |
| 95 | + package="ros_gz_sim", |
| 96 | + executable="create", |
| 97 | + arguments=[ |
| 98 | + "-topic", |
| 99 | + "robot_description", |
| 100 | + "-name", |
| 101 | + LaunchConfiguration("robot_name"), |
| 102 | + "-allow_renaming", |
| 103 | + "-x", |
| 104 | + "0.0", |
| 105 | + "-y", |
| 106 | + "0.0", |
| 107 | + "-z", |
| 108 | + "0.0", |
| 109 | + "-R", |
| 110 | + "0.0", |
| 111 | + "-P", |
| 112 | + "0.0", |
| 113 | + "-Y", |
| 114 | + "0.0", |
| 115 | + ], |
| 116 | + output="screen", |
| 117 | + namespace=LaunchConfiguration("robot_name"), |
| 118 | + ), # spawns robot in Gazebo through robot_description topic of robot_state_publisher |
| 119 | + Node( |
| 120 | + package="controller_manager", |
| 121 | + executable="spawner", |
| 122 | + output="screen", |
| 123 | + arguments=[ |
| 124 | + "--controller-manager", |
| 125 | + "controller_manager", |
| 126 | + "joint_state_broadcaster", |
| 127 | + LaunchConfiguration("ctrl"), |
| 128 | + ], |
| 129 | + namespace=LaunchConfiguration("robot_name"), |
| 130 | + ), |
| 131 | + ] |
42 | 132 | ) |
43 | | - return ld |
|
0 commit comments