|
1 | 1 | #! /usr/bin/env python3 |
2 | | -""" |
3 | | -Spawn Robot in Gazebo |
4 | | -""" |
| 2 | +"""Script to launch Gazebo with RViz support for the Atreus robot.""" |
| 3 | + |
5 | 4 | import os |
| 5 | + |
6 | 6 | from ament_index_python.packages import get_package_share_directory |
7 | | -from launch import LaunchDescription |
8 | | -from launch.launch_description_sources import PythonLaunchDescriptionSource |
9 | | -from launch.substitutions import LaunchConfiguration, Command, PythonExpression, PathJoinSubstitution |
10 | | -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, AppendEnvironmentVariable |
11 | | -from launch.conditions import IfCondition |
12 | 7 | from launch_ros.actions import Node |
13 | 8 | from launch_ros.substitutions import FindPackageShare |
14 | 9 |
|
| 10 | +from launch import LaunchDescription |
| 11 | +from launch.actions import ( |
| 12 | + AppendEnvironmentVariable, |
| 13 | + DeclareLaunchArgument, |
| 14 | + IncludeLaunchDescription, |
| 15 | +) |
| 16 | +from launch.conditions import IfCondition |
| 17 | +from launch.launch_description_sources import PythonLaunchDescriptionSource |
| 18 | +from launch.substitutions import ( |
| 19 | + Command, |
| 20 | + LaunchConfiguration, |
| 21 | + PathJoinSubstitution, |
| 22 | + PythonExpression, |
| 23 | +) |
| 24 | + |
15 | 25 |
|
16 | 26 | def generate_launch_description(): |
17 | | - """ |
18 | | - Launch Function |
19 | | - """ |
20 | | - pkg_dir = get_package_share_directory('atreus') |
21 | | - |
22 | | - rviz_config_path = os.path.join( |
23 | | - pkg_dir, 'config', 'rviz', 'urdf.rviz') |
24 | | - world_path = os.path.join(pkg_dir, 'worlds', 'mapping.sdf') |
25 | | - bridge_params_path = os.path.join( |
26 | | - pkg_dir, 'config', 'ros_gz_bridge.yaml' |
27 | | - ) |
| 27 | + """Generate the launch description for Atreus.""" |
| 28 | + pkg_dir = get_package_share_directory("atreus") |
28 | 29 |
|
| 30 | + rviz_config_path = os.path.join(pkg_dir, "config", "rviz", "urdf.rviz") |
| 31 | + world_path = os.path.join(pkg_dir, "worlds", "mapping.sdf") |
| 32 | + bridge_params_path = os.path.join(pkg_dir, "config", "ros_gz_bridge.yaml") |
29 | 33 |
|
30 | | - # Create the launch configuration variables |
31 | | - use_sim_time = LaunchConfiguration('use_sim_time') |
32 | | - camera_enabled = LaunchConfiguration('camera_enabled') |
33 | | - two_d_lidar_enabled = LaunchConfiguration('two_d_lidar_enabled') |
34 | | - rviz_enabled = LaunchConfiguration('rviz_enabled') |
35 | | - rviz_config = LaunchConfiguration('rviz_config') |
| 34 | + # Create the launch configuration variables |
| 35 | + use_sim_time = LaunchConfiguration("use_sim_time") |
| 36 | + camera_enabled = LaunchConfiguration("camera_enabled") |
| 37 | + two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled") |
| 38 | + rviz_enabled = LaunchConfiguration("rviz_enabled") |
| 39 | + rviz_config = LaunchConfiguration("rviz_config") |
36 | 40 |
|
37 | 41 | # Declare the append environment variables |
38 | 42 | append_env_var_gz_sim_resource_path = AppendEnvironmentVariable( |
39 | | - name='GZ_SIM_RESOURCE_PATH', |
40 | | - value=os.path.join(pkg_dir, "worlds") + ':' + |
41 | | - os.path.join(pkg_dir, "models", "warehouse_models") |
| 43 | + name="GZ_SIM_RESOURCE_PATH", |
| 44 | + value=os.path.join(pkg_dir, "worlds") |
| 45 | + + ":" |
| 46 | + + os.path.join(pkg_dir, "models", "warehouse_models"), |
42 | 47 | ) |
43 | 48 |
|
44 | 49 | # Declare the launch arguments |
45 | 50 | declare_use_sim_time_arg = DeclareLaunchArgument( |
46 | | - 'use_sim_time', default_value='True', |
47 | | - description='Flag to enable use_sim_time' |
| 51 | + "use_sim_time", default_value="True", description="Flag to enable use_sim_time" |
48 | 52 | ) |
49 | 53 | declare_world_name_arg = DeclareLaunchArgument( |
50 | | - 'world_name', default_value=world_path, |
51 | | - description='Choice of Gazebo World' |
| 54 | + "world_name", default_value=world_path, description="Choice of Gazebo World" |
52 | 55 | ) |
53 | 56 | declare_camera_enabled_arg = DeclareLaunchArgument( |
54 | | - 'camera_enabled', default_value='False', |
55 | | - description='Flag to enable camera' |
| 57 | + "camera_enabled", default_value="False", description="Flag to enable camera" |
56 | 58 | ) |
57 | 59 | declare_two_d_lidar_enabled_arg = DeclareLaunchArgument( |
58 | | - 'two_d_lidar_enabled', default_value='False', |
59 | | - description='Flag to enable 2D LiDAR' |
| 60 | + "two_d_lidar_enabled", |
| 61 | + default_value="False", |
| 62 | + description="Flag to enable 2D LiDAR", |
60 | 63 | ) |
61 | 64 | declare_rviz_enabled_arg = DeclareLaunchArgument( |
62 | | - 'rviz_enabled', default_value='False', |
63 | | - description='Flag to enable RViz' |
| 65 | + "rviz_enabled", default_value="False", description="Flag to enable RViz" |
64 | 66 | ) |
65 | 67 | declare_rviz_config_arg = DeclareLaunchArgument( |
66 | | - 'rviz_config', default_value=rviz_config_path, |
67 | | - description='Full path to the RViz config file to use' |
| 68 | + "rviz_config", |
| 69 | + default_value=rviz_config_path, |
| 70 | + description="Full path to the RViz config file to use", |
68 | 71 | ) |
69 | 72 |
|
70 | 73 | # Include the Nodes |
71 | 74 | gz_spawn_entity_node = Node( |
72 | 75 | package="ros_gz_sim", |
73 | 76 | executable="create", |
74 | 77 | arguments=[ |
75 | | - "-topic", "/robot_description", |
76 | | - "-name", "atreus", |
77 | | - "-x", "0", |
78 | | - "-y", "0", |
79 | | - "-z", "0.5", |
80 | | - ] |
| 78 | + "-topic", |
| 79 | + "/robot_description", |
| 80 | + "-name", |
| 81 | + "atreus", |
| 82 | + "-x", |
| 83 | + "0", |
| 84 | + "-y", |
| 85 | + "0", |
| 86 | + "-z", |
| 87 | + "0.5", |
| 88 | + ], |
81 | 89 | ) |
82 | 90 | gz_ros2_bridge_node = Node( |
83 | | - package='ros_gz_bridge', |
84 | | - executable='parameter_bridge', |
85 | | - name='bridge_ros_gz', |
86 | | - parameters=[{ |
87 | | - 'config_file': bridge_params_path, |
88 | | - 'use_sim_time': use_sim_time |
89 | | - }], |
90 | | - output='screen', |
| 91 | + package="ros_gz_bridge", |
| 92 | + executable="parameter_bridge", |
| 93 | + name="bridge_ros_gz", |
| 94 | + parameters=[{"config_file": bridge_params_path, "use_sim_time": use_sim_time}], |
| 95 | + output="screen", |
91 | 96 | ) |
92 | 97 |
|
93 | 98 | robot_state_publisher_node = Node( |
94 | | - package='robot_state_publisher', |
95 | | - executable='robot_state_publisher', |
96 | | - parameters=[{'robot_description': Command( |
97 | | - ['xacro ', os.path.join(pkg_dir, 'urdf', 'atreus.xacro'), |
98 | | - ' camera_enabled:=', camera_enabled, |
99 | | - ' two_d_lidar_enabled:=', two_d_lidar_enabled |
100 | | - ])}] |
| 99 | + package="robot_state_publisher", |
| 100 | + executable="robot_state_publisher", |
| 101 | + parameters=[ |
| 102 | + { |
| 103 | + "robot_description": Command( |
| 104 | + [ |
| 105 | + "xacro ", |
| 106 | + os.path.join(pkg_dir, "urdf", "atreus.xacro"), |
| 107 | + " camera_enabled:=", |
| 108 | + camera_enabled, |
| 109 | + " two_d_lidar_enabled:=", |
| 110 | + two_d_lidar_enabled, |
| 111 | + ] |
| 112 | + ) |
| 113 | + } |
| 114 | + ], |
101 | 115 | ) |
102 | 116 |
|
103 | 117 | gz_sim_launch = IncludeLaunchDescription( |
104 | 118 | PythonLaunchDescriptionSource( |
105 | 119 | PathJoinSubstitution( |
106 | | - [FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'])), |
| 120 | + [FindPackageShare("ros_gz_sim"), "launch", "gz_sim.launch.py"] |
| 121 | + ) |
| 122 | + ), |
107 | 123 | launch_arguments={ |
108 | | - "gz_args" : PythonExpression( |
109 | | - ["'", world_path, " -r'"]) |
110 | | - }.items() |
| 124 | + "gz_args": PythonExpression(["'", world_path, " -r'"]) |
| 125 | + }.items(), |
111 | 126 | ) |
112 | 127 |
|
113 | 128 | rviz_launch = IncludeLaunchDescription( |
114 | 129 | PythonLaunchDescriptionSource( |
115 | | - os.path.join(pkg_dir, "launch", "description", "rviz.launch.py")), |
| 130 | + os.path.join(pkg_dir, "launch", "description", "rviz.launch.py") |
| 131 | + ), |
116 | 132 | launch_arguments={ |
117 | | - 'gazebo_enabled': 'True', |
118 | | - 'camera_enabled': camera_enabled, |
119 | | - 'two_d_lidar_enabled': two_d_lidar_enabled, |
120 | | - 'rviz_config': rviz_config, |
121 | | - }.items(), |
122 | | - condition=IfCondition(rviz_enabled) |
| 133 | + "gazebo_enabled": "True", |
| 134 | + "camera_enabled": camera_enabled, |
| 135 | + "two_d_lidar_enabled": two_d_lidar_enabled, |
| 136 | + "rviz_config": rviz_config, |
| 137 | + }.items(), |
| 138 | + condition=IfCondition(rviz_enabled), |
123 | 139 | ) |
124 | 140 |
|
125 | 141 | ld = LaunchDescription() |
|
0 commit comments