SMACC2
sm_dance_bot_warehouse_3_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_3_dir = get_package_share_directory("sm_dance_bot_warehouse_3")
32 sm_dance_bot_warehouse_3_launch_dir = os.path.join(sm_dance_bot_warehouse_3_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 headless = LaunchConfiguration("headless")
52
53 urdf = os.path.join(sm_dance_bot_warehouse_3_dir, "urdf", "turtlebot3_waffle.urdf")
54
55 # Map fully qualified names to relative ones so the node's namespace can be prepended.
56 # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
57 # https://github.com/ros/geometry2/issues/32
58 # https://github.com/ros/robot_state_publisher/pull/30
59 # TODO(orduno) Substitute with `PushNodeRemapping`
60 # https://github.com/ros2/launch_ros/issues/56
61 remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
62
63 # Declare the launch arguments
64 declare_namespace_cmd = DeclareLaunchArgument(
65 "namespace", default_value="", description="Top-level namespace"
66 )
67
68 declare_headless_simulator_argument = DeclareLaunchArgument(
69 "headless", default_value="False", description="Whether to execute gzclient)"
70 )
71
72 declare_use_namespace_cmd = DeclareLaunchArgument(
73 "use_namespace",
74 default_value="false",
75 description="Whether to apply a namespace to the navigation stack",
76 )
77
78 declare_show_gz_lidar = DeclareLaunchArgument(
79 "show_gz_lidar",
80 default_value="true",
81 description="Whether to apply a namespace to the navigation stack",
82 )
83
84 declare_slam_cmd = DeclareLaunchArgument(
85 "slam", default_value="True", description="Whether run a SLAM"
86 )
87
88 declare_use_sim_time_cmd = DeclareLaunchArgument(
89 "use_sim_time", default_value="true", description="Use simulation (Gazebo) clock if true"
90 )
91
92 declare_params_file_cmd = DeclareLaunchArgument(
93 "params_file",
94 default_value=os.path.join(
95 sm_dance_bot_warehouse_3_dir, "params", "nav2z_client", "nav2_params.yaml"
96 ),
97 description="Full path to the ROS2 parameters file to use for all launched nodes",
98 )
99
100 declare_bt_xml_cmd = DeclareLaunchArgument(
101 "default_nav_to_pose_bt_xml",
102 default_value=os.path.join(
103 sm_dance_bot_warehouse_3_dir, "params", "nav2z_client", "navigation_tree.xml"
104 ),
105 description="Full path to the behavior tree xml file to use",
106 )
107
108 declare_autostart_cmd = DeclareLaunchArgument(
109 "autostart", default_value="true", description="Automatically startup the nav2 stack"
110 )
111
112 declare_rviz_config_file_cmd = DeclareLaunchArgument(
113 "rviz_config_file",
114 default_value=os.path.join(sm_dance_bot_warehouse_3_dir, "rviz", "nav2_default_view.rviz"),
115 description="Full path to the RVIZ config file to use",
116 )
117
118 declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
119 "use_robot_state_pub",
120 default_value="True",
121 description="Whether to start the robot state publisher",
122 )
123
124 declare_use_rviz_cmd = DeclareLaunchArgument(
125 "use_rviz", default_value="True", description="Whether to start RVIZ"
126 )
127
128 declare_map_yaml_cmd = DeclareLaunchArgument(
129 "map",
130 default_value=os.path.join(sm_dance_bot_warehouse_3_dir, "maps", "turtlebot3_world.yaml"),
131 description="Full path to map file to load",
132 )
133
134 start_robot_state_publisher_cmd = Node(
135 condition=IfCondition(use_robot_state_pub),
136 package="robot_state_publisher",
137 executable="robot_state_publisher",
138 name="robot_state_publisher",
139 namespace=namespace,
140 output="screen",
141 parameters=[{"use_sim_time": use_sim_time}],
142 remappings=remappings,
143 arguments=[urdf],
144 )
145
146 rviz_cmd = IncludeLaunchDescription(
147 PythonLaunchDescriptionSource(
148 os.path.join(sm_dance_bot_warehouse_3_launch_dir, "rviz_launch.py")
149 ),
150 condition=IfCondition(use_rviz),
151 launch_arguments={
152 "namespace": "",
153 "use_namespace": "False",
154 "rviz_config": rviz_config_file,
155 }.items(),
156 )
157
158 bringup_cmd = IncludeLaunchDescription(
159 PythonLaunchDescriptionSource(
160 os.path.join(sm_dance_bot_warehouse_3_launch_dir, "bringup_launch.py")
161 ),
162 launch_arguments={
163 "namespace": namespace,
164 "use_namespace": use_namespace,
165 "autostart": autostart,
166 "params_file": params_file,
167 "slam": slam,
168 "map": map_yaml_file,
169 "use_sim_time": use_sim_time,
170 "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml,
171 }.items(),
172 )
173
174 bringup_cmd = IncludeLaunchDescription(
175 PythonLaunchDescriptionSource(
176 os.path.join(sm_dance_bot_warehouse_3_launch_dir, "bringup_launch.py")
177 ),
178 launch_arguments={
179 "namespace": namespace,
180 "use_namespace": use_namespace,
181 "autostart": autostart,
182 "params_file": params_file,
183 "slam": slam,
184 "map": map_yaml_file,
185 "use_sim_time": use_sim_time,
186 "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml,
187 }.items(),
188 )
189
190 # gazebo_husky = IncludeLaunchDescription(
191 # PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_warehouse_3_launch_dir, "husky_gazebo.launch.py")),
192 # launch_arguments={'world_path': os.path.join(sm_dance_bot_warehouse_3_dir, "worlds", "ridgeback_race_empty.world")}.items()
193 # )
194
195 gazebo_simulator = IncludeLaunchDescription(
196 PythonLaunchDescriptionSource(
197 os.path.join(sm_dance_bot_warehouse_3_launch_dir, "gazebo_launch.py")
198 ),
199 launch_arguments={"show_gz_lidar": show_gz_lidar, "headless": headless}.items(),
200 )
201
202 xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e"
203
204 sm_dance_bot_warehouse_3_node = Node(
205 package="sm_dance_bot_warehouse_3",
206 executable="sm_dance_bot_warehouse_3_node",
207 name="SmDanceBotWareHouse3",
208 output="screen",
209 prefix=xtermprefix,
210 parameters=[
211 os.path.join(
212 get_package_share_directory("sm_dance_bot_warehouse_3"),
213 "params/sm_dance_bot_warehouse_3_config.yaml",
214 ),
215 os.path.join(
216 get_package_share_directory("sm_dance_bot_warehouse_3"),
217 "params/nav2z_client/odom_tracker.yaml",
218 ),
219 ],
220 remappings=[
221 # ("/odom", "/odometry/filtered"),
222 # ("/sm_dance_bot_warehouse_3_2/odom_tracker/odom_tracker_path", "/odom_tracker_path"),
223 # ("/sm_dance_bot_warehouse_3_2/odom_tracker/odom_tracker_stacked_path", "/odom_tracker_path_stacked")
224 ],
225 arguments=["--ros-args", "--log-level", "INFO"],
226 )
227
228 led_action_server_node = Node(
229 package="sm_dance_bot_warehouse_3",
230 executable="led_action_server_node",
231 output="screen",
232 prefix=xtermprefix,
233 )
234
235 temperature_action_server = Node(
236 package="sm_dance_bot_warehouse_3",
237 executable="temperature_sensor_node",
238 output="screen",
239 prefix=xtermprefix,
240 )
241
242 service3_node = Node(
243 package="sm_dance_bot_warehouse_3",
244 executable="service_node_3.py",
245 output="screen",
246 prefix=xtermprefix,
247 parameters=[
248 {"autostart": True, "node_names": ["ss", "dfa"]},
249 ],
250 )
251
252 # Create the launch description and populate
253 ld = LaunchDescription()
254
255 # Declare the launch options
256 ld.add_action(declare_namespace_cmd)
257 ld.add_action(declare_use_namespace_cmd)
258 ld.add_action(declare_slam_cmd)
259 ld.add_action(declare_use_sim_time_cmd)
260 ld.add_action(declare_params_file_cmd)
261 ld.add_action(declare_bt_xml_cmd)
262 ld.add_action(declare_autostart_cmd)
263 ld.add_action(declare_map_yaml_cmd)
264 ld.add_action(declare_show_gz_lidar)
265 ld.add_action(declare_headless_simulator_argument)
266
267 ld.add_action(declare_rviz_config_file_cmd)
268 ld.add_action(declare_use_robot_state_pub_cmd)
269 ld.add_action(declare_use_rviz_cmd)
270 ld.add_action(gazebo_simulator)
271 # ld.add_action(gazebo_husky)
272
273 ld.add_action(sm_dance_bot_warehouse_3_node)
274 ld.add_action(service3_node)
275 ld.add_action(temperature_action_server)
276 ld.add_action(led_action_server_node)
277
278 # # Add the actions to launch all of the navigation nodes
279 ld.add_action(start_robot_state_publisher_cmd)
280 ld.add_action(rviz_cmd)
281 ld.add_action(bringup_cmd)
282
283 return ld