SMACC2
sm_dance_bot_warehouse_launch.py
Go to the documentation of this file.
1# Copyright (c) 2018 Intel Corporation
2#
3# Licensed under the Apache License, Version 2.0 (the "License");
4# you may not use this file except in compliance with the License.
5# You may obtain a copy of the License at
6#
7# http://www.apache.org/licenses/LICENSE-2.0
8#
9# Unless required by applicable law or agreed to in writing, software
10# distributed under the License is distributed on an "AS IS" BASIS,
11# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12# See the License for the specific language governing permissions and
13# limitations under the License.
14
15"""This is all-in-one launch script intended for use by nav2 developers."""
16
17import os
18
19from ament_index_python.packages import get_package_share_directory
20
21from launch import LaunchDescription
22from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
23from launch.conditions import IfCondition
24from launch.launch_description_sources import PythonLaunchDescriptionSource
25from launch.substitutions import LaunchConfiguration
26from launch_ros.actions import Node
27
28
30 # Get the launch directory
31 sm_dance_bot_warehouse_dir = get_package_share_directory("sm_dance_bot_warehouse")
32 sm_dance_bot_warehouse_launch_dir = os.path.join(sm_dance_bot_warehouse_dir, "launch")
33
34 # Create the launch configuration variables
35 slam = LaunchConfiguration("slam")
36 namespace = LaunchConfiguration("namespace")
37 use_namespace = LaunchConfiguration("use_namespace")
38 map_yaml_file = LaunchConfiguration("map")
39 use_sim_time = LaunchConfiguration("use_sim_time")
40 params_file = LaunchConfiguration("params_file")
41 default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml")
42 autostart = LaunchConfiguration("autostart")
43 show_gz_lidar = LaunchConfiguration("show_gz_lidar")
44 headless = LaunchConfiguration("headless")
45
46 # Launch configuration variables specific to simulation
47 rviz_config_file = LaunchConfiguration("rviz_config_file")
48
49 use_robot_state_pub = LaunchConfiguration("use_robot_state_pub")
50 use_rviz = LaunchConfiguration("use_rviz")
51
52 urdf = os.path.join(sm_dance_bot_warehouse_dir, "urdf", "turtlebot3_waffle.urdf")
53
54 # Map fully qualified names to relative ones so the node's namespace can be prepended.
55 # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
56 # https://github.com/ros/geometry2/issues/32
57 # https://github.com/ros/robot_state_publisher/pull/30
58 # TODO(orduno) Substitute with `PushNodeRemapping`
59 # https://github.com/ros2/launch_ros/issues/56
60 remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
61
62 # Declare the launch arguments
63 declare_namespace_cmd = DeclareLaunchArgument(
64 "namespace", default_value="", description="Top-level namespace"
65 )
66
67 declare_headless_simulator_argument = DeclareLaunchArgument(
68 "headless", default_value="False", description="Whether to execute gzclient)"
69 )
70
71 declare_use_namespace_cmd = DeclareLaunchArgument(
72 "use_namespace",
73 default_value="false",
74 description="Whether to apply a namespace to the navigation stack",
75 )
76
77 declare_show_gz_lidar = DeclareLaunchArgument(
78 "show_gz_lidar",
79 default_value="true",
80 description="Whether to apply a namespace to the navigation stack",
81 )
82
83 declare_slam_cmd = DeclareLaunchArgument(
84 "slam", default_value="True", description="Whether run a SLAM"
85 )
86
87 declare_use_sim_time_cmd = DeclareLaunchArgument(
88 "use_sim_time", default_value="true", description="Use simulation (Gazebo) clock if true"
89 )
90
91 declare_params_file_cmd = DeclareLaunchArgument(
92 "params_file",
93 default_value=os.path.join(
94 sm_dance_bot_warehouse_dir, "params", "nav2z_client", "nav2_params.yaml"
95 ),
96 description="Full path to the ROS2 parameters file to use for all launched nodes",
97 )
98
99 declare_bt_xml_cmd = DeclareLaunchArgument(
100 "default_nav_to_pose_bt_xml",
101 default_value=os.path.join(
102 sm_dance_bot_warehouse_dir, "params", "nav2z_client", "navigation_tree.xml"
103 ),
104 description="Full path to the behavior tree xml file to use",
105 )
106
107 declare_autostart_cmd = DeclareLaunchArgument(
108 "autostart", default_value="true", description="Automatically startup the nav2 stack"
109 )
110
111 declare_rviz_config_file_cmd = DeclareLaunchArgument(
112 "rviz_config_file",
113 default_value=os.path.join(sm_dance_bot_warehouse_dir, "rviz", "nav2_default_view.rviz"),
114 description="Full path to the RVIZ config file to use",
115 )
116
117 declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
118 "use_robot_state_pub",
119 default_value="True",
120 description="Whether to start the robot state publisher",
121 )
122
123 declare_use_rviz_cmd = DeclareLaunchArgument(
124 "use_rviz", default_value="True", description="Whether to start RVIZ"
125 )
126
127 declare_map_yaml_cmd = DeclareLaunchArgument(
128 "map",
129 default_value=os.path.join(sm_dance_bot_warehouse_dir, "maps", "turtlebot3_world.yaml"),
130 description="Full path to map file to load",
131 )
132
133 start_robot_state_publisher_cmd = Node(
134 condition=IfCondition(use_robot_state_pub),
135 package="robot_state_publisher",
136 executable="robot_state_publisher",
137 name="robot_state_publisher",
138 namespace=namespace,
139 output="screen",
140 parameters=[{"use_sim_time": use_sim_time}],
141 remappings=remappings,
142 arguments=[urdf],
143 )
144
145 rviz_cmd = IncludeLaunchDescription(
146 PythonLaunchDescriptionSource(
147 os.path.join(sm_dance_bot_warehouse_launch_dir, "rviz_launch.py")
148 ),
149 condition=IfCondition(use_rviz),
150 launch_arguments={
151 "namespace": "",
152 "use_namespace": "False",
153 "rviz_config": rviz_config_file,
154 }.items(),
155 )
156
157 bringup_cmd = IncludeLaunchDescription(
158 PythonLaunchDescriptionSource(
159 os.path.join(sm_dance_bot_warehouse_launch_dir, "bringup_launch.py")
160 ),
161 launch_arguments={
162 "namespace": namespace,
163 "use_namespace": use_namespace,
164 "autostart": autostart,
165 "params_file": params_file,
166 "slam": slam,
167 "map": map_yaml_file,
168 "use_sim_time": use_sim_time,
169 "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml,
170 }.items(),
171 )
172
173 bringup_cmd = IncludeLaunchDescription(
174 PythonLaunchDescriptionSource(
175 os.path.join(sm_dance_bot_warehouse_launch_dir, "bringup_launch.py")
176 ),
177 launch_arguments={
178 "namespace": namespace,
179 "use_namespace": use_namespace,
180 "autostart": autostart,
181 "params_file": params_file,
182 "slam": slam,
183 "map": map_yaml_file,
184 "use_sim_time": use_sim_time,
185 "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml,
186 }.items(),
187 )
188
189 # gazebo_husky = IncludeLaunchDescription(
190 # PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_warehouse_launch_dir, "husky_gazebo.launch.py")),
191 # launch_arguments={'world_path': os.path.join(sm_dance_bot_warehouse_dir, "worlds", "ridgeback_race_empty.world")}.items()
192 # )
193
194 gazebo_simulator = IncludeLaunchDescription(
195 PythonLaunchDescriptionSource(
196 os.path.join(sm_dance_bot_warehouse_launch_dir, "gazebo_launch.py")
197 ),
198 launch_arguments={"show_gz_lidar": show_gz_lidar, "headless": headless}.items(),
199 )
200
201 xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e"
202
203 sm_dance_bot_warehouse_node = Node(
204 package="sm_dance_bot_warehouse",
205 executable="sm_dance_bot_warehouse_node",
206 name="SmDanceBotWarehouse",
207 output="screen",
208 prefix=xtermprefix,
209 parameters=[
210 os.path.join(
211 get_package_share_directory("sm_dance_bot_warehouse"),
212 "params/sm_dance_bot_warehouse_config.yaml",
213 )
214 ],
215 remappings=[
216 # ("/odom", "/odometry/filtered"),
217 # ("/sm_dance_bot_warehouse_2/odom_tracker/odom_tracker_path", "/odom_tracker_path"),
218 # ("/sm_dance_bot_warehouse_2/odom_tracker/odom_tracker_stacked_path", "/odom_tracker_path_stacked")
219 ],
220 arguments=["--ros-args", "--log-level", "INFO"],
221 )
222
223 led_action_server_node = Node(
224 package="sm_dance_bot_warehouse",
225 executable="led_action_server_node",
226 output="screen",
227 prefix=xtermprefix,
228 )
229
230 temperature_action_server = Node(
231 package="sm_dance_bot_warehouse",
232 executable="temperature_sensor_node",
233 output="screen",
234 prefix=xtermprefix,
235 )
236
237 service3_node = Node(
238 package="sm_dance_bot_warehouse",
239 executable="service_node_3.py",
240 output="screen",
241 prefix=xtermprefix,
242 parameters=[
243 {"autostart": True, "node_names": ["ss", "dfa"]},
244 ],
245 )
246
247 # Create the launch description and populate
248 ld = LaunchDescription()
249
250 # Declare the launch options
251 ld.add_action(declare_namespace_cmd)
252 ld.add_action(declare_use_namespace_cmd)
253 ld.add_action(declare_slam_cmd)
254 ld.add_action(declare_use_sim_time_cmd)
255 ld.add_action(declare_params_file_cmd)
256 ld.add_action(declare_bt_xml_cmd)
257 ld.add_action(declare_autostart_cmd)
258 ld.add_action(declare_map_yaml_cmd)
259 ld.add_action(declare_show_gz_lidar)
260 ld.add_action(declare_headless_simulator_argument)
261
262 ld.add_action(declare_rviz_config_file_cmd)
263 ld.add_action(declare_use_robot_state_pub_cmd)
264 ld.add_action(declare_use_rviz_cmd)
265 ld.add_action(gazebo_simulator)
266 # ld.add_action(gazebo_husky)
267
268 ld.add_action(sm_dance_bot_warehouse_node)
269 ld.add_action(service3_node)
270 ld.add_action(temperature_action_server)
271 ld.add_action(led_action_server_node)
272
273 # # Add the actions to launch all of the navigation nodes
274 ld.add_action(start_robot_state_publisher_cmd)
275 ld.add_action(rviz_cmd)
276 ld.add_action(bringup_cmd)
277
278 return ld