SMACC2
sm_dance_bot_warehouse_2_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_2_dir = get_package_share_directory("sm_dance_bot_warehouse_2")
32 sm_dance_bot_warehouse_2_launch_dir = os.path.join(sm_dance_bot_warehouse_2_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_2_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_2_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_2_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_2_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_2_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_2_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_2_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_2_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_2_launch_dir, "husky_gazebo.launch.py")),
191 # launch_arguments={'world_path': os.path.join(sm_dance_bot_warehouse_2_dir, "worlds", "ridgeback_race_empty.world")}.items()
192 # )
193
194 gazebo_simulator = IncludeLaunchDescription(
195 PythonLaunchDescriptionSource(
196 os.path.join(sm_dance_bot_warehouse_2_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_2_node = Node(
204 package="sm_dance_bot_warehouse_2",
205 executable="sm_dance_bot_warehouse_2_node",
206 name="SmDanceBotWareHouse2",
207 output="screen",
208 prefix=xtermprefix,
209 parameters=[
210 os.path.join(
211 get_package_share_directory("sm_dance_bot_warehouse_2"),
212 "params/sm_dance_bot_warehouse_2_config.yaml",
213 ),
214 os.path.join(
215 get_package_share_directory("sm_dance_bot_warehouse_2"),
216 "params/nav2z_client/odom_tracker.yaml",
217 ),
218 ],
219 remappings=[
220 # ("/odom", "/odometry/filtered"),
221 # ("/sm_dance_bot_warehouse_2_2/odom_tracker/odom_tracker_path", "/odom_tracker_path"),
222 # ("/sm_dance_bot_warehouse_2_2/odom_tracker/odom_tracker_stacked_path", "/odom_tracker_path_stacked")
223 ],
224 arguments=["--ros-args", "--log-level", "INFO"],
225 )
226
227 led_action_server_node = Node(
228 package="sm_dance_bot_warehouse_2",
229 executable="led_action_server_node",
230 output="screen",
231 prefix=xtermprefix,
232 )
233
234 temperature_action_server = Node(
235 package="sm_dance_bot_warehouse_2",
236 executable="temperature_sensor_node",
237 output="screen",
238 prefix=xtermprefix,
239 )
240
241 service3_node = Node(
242 package="sm_dance_bot_warehouse_2",
243 executable="service_node_3.py",
244 output="screen",
245 prefix=xtermprefix,
246 parameters=[
247 {"autostart": True, "node_names": ["ss", "dfa"]},
248 ],
249 )
250
251 # Create the launch description and populate
252 ld = LaunchDescription()
253
254 # Declare the launch options
255 ld.add_action(declare_namespace_cmd)
256 ld.add_action(declare_use_namespace_cmd)
257 ld.add_action(declare_slam_cmd)
258 ld.add_action(declare_use_sim_time_cmd)
259 ld.add_action(declare_params_file_cmd)
260 ld.add_action(declare_bt_xml_cmd)
261 ld.add_action(declare_autostart_cmd)
262 ld.add_action(declare_map_yaml_cmd)
263 ld.add_action(declare_show_gz_lidar)
264 ld.add_action(declare_headless_simulator_argument)
265
266 ld.add_action(declare_rviz_config_file_cmd)
267 ld.add_action(declare_use_robot_state_pub_cmd)
268 ld.add_action(declare_use_rviz_cmd)
269 ld.add_action(gazebo_simulator)
270 # ld.add_action(gazebo_husky)
271
272 ld.add_action(sm_dance_bot_warehouse_2_node)
273 ld.add_action(service3_node)
274 ld.add_action(temperature_action_server)
275 ld.add_action(led_action_server_node)
276
277 # # Add the actions to launch all of the navigation nodes
278 ld.add_action(start_robot_state_publisher_cmd)
279 ld.add_action(rviz_cmd)
280 ld.add_action(bringup_cmd)
281
282 return ld