SMACC2
sm_husky_barrel_search_1_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_husky_barrel_search_1_dir = get_package_share_directory("sm_husky_barrel_search_1")
32 sm_husky_barrel_search_1_launch_dir = os.path.join(sm_husky_barrel_search_1_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
45 # Launch configuration variables specific to simulation
46 rviz_config_file = LaunchConfiguration("rviz_config_file")
47
48 use_robot_state_pub = LaunchConfiguration("use_robot_state_pub")
49 use_rviz = LaunchConfiguration("use_rviz")
50 # headless = LaunchConfiguration("headless")
51
52 urdf = os.path.join(sm_husky_barrel_search_1_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_husky_barrel_search_1_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_husky_barrel_search_1_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_husky_barrel_search_1_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_husky_barrel_search_1_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_husky_barrel_search_1_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_husky_barrel_search_1_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 ekf_gps_localization = IncludeLaunchDescription(
174 PythonLaunchDescriptionSource(
175 os.path.join(sm_husky_barrel_search_1_launch_dir, "ekf_gps_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_simulator = IncludeLaunchDescription(
190 PythonLaunchDescriptionSource(
191 os.path.join(sm_husky_barrel_search_1_launch_dir, "gazebo_launch.py")
192 ),
193 launch_arguments={
194 "world": os.path.join(
195 sm_husky_barrel_search_1_dir, "worlds", "barrel_valley.world.sdf"
196 )
197 }.items(),
198 )
199
200 # gazebo_simulator = IncludeLaunchDescription(
201 # PythonLaunchDescriptionSource(
202 # os.path.join(sm_husky_barrel_search_1_launch_dir, "gazebo_launch.py")
203 # ),
204 # launch_arguments={"show_gz_lidar": show_gz_lidar, "headless": headless}.items(),
205 # )
206
207 xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e"
208
209 sm_husky_barrel_search_1_node = Node(
210 package="sm_husky_barrel_search_1",
211 executable="sm_husky_barrel_search_1_node",
212 name="SmHuskyBarrelSearch1",
213 output="screen",
214 prefix=xtermprefix,
215 parameters=[
216 os.path.join(
217 get_package_share_directory("sm_husky_barrel_search_1"),
218 "params/sm_husky_barrel_search_1_config.yaml",
219 ),
220 os.path.join(
221 get_package_share_directory("sm_husky_barrel_search_1"),
222 "params/nav2z_client/odom_tracker.yaml",
223 ),
224 ],
225 remappings=[
226 # ("/odom", "/odometry/filtered"),
227 # ("/sm_husky_barrel_search_1_2/odom_tracker/odom_tracker_path", "/odom_tracker_path"),
228 # ("/sm_husky_barrel_search_1_2/odom_tracker/odom_tracker_stacked_path", "/odom_tracker_path_stacked")
229 ],
230 arguments=["--ros-args", "--log-level", "INFO"],
231 )
232
233 # led_action_server_node = Node(
234 # package="sm_husky_barrel_search_1",
235 # executable="led_action_server_node",
236 # output="screen",
237 # prefix=xtermprefix,
238 # )
239
240 # temperature_action_server = Node(
241 # package="sm_husky_barrel_search_1",
242 # executable="temperature_sensor_node",
243 # output="screen",
244 # prefix=xtermprefix,
245 # )
246
247 # service3_node = Node(
248 # package="sm_husky_barrel_search_1",
249 # executable="service_node_3.py",
250 # output="screen",
251 # prefix=xtermprefix,
252 # parameters=[
253 # {"autostart": True, "node_names": ["ss", "dfa"]},
254 # ],
255 # )
256
257 # Create the launch description and populate
258 ld = LaunchDescription()
259
260 # Declare the launch options
261 ld.add_action(declare_namespace_cmd)
262 ld.add_action(declare_use_namespace_cmd)
263 # ld.add_action(declare_slam_cmd)
264 ld.add_action(declare_use_sim_time_cmd)
265 ld.add_action(declare_params_file_cmd)
266 ld.add_action(declare_bt_xml_cmd)
267 ld.add_action(declare_autostart_cmd)
268 ld.add_action(declare_map_yaml_cmd)
269 ld.add_action(declare_show_gz_lidar)
270 ld.add_action(declare_headless_simulator_argument)
271
272 ld.add_action(declare_rviz_config_file_cmd)
273 ld.add_action(declare_use_robot_state_pub_cmd)
274 ld.add_action(declare_use_rviz_cmd)
275 ld.add_action(gazebo_simulator)
276
277 # ld.add_action(gazebo_husky)
278
279 ld.add_action(sm_husky_barrel_search_1_node)
280 # ld.add_action(service3_node)
281 # ld.add_action(temperature_action_server)
282 # ld.add_action(led_action_server_node)
283
284 # # Add the actions to launch all of the navigation nodes
285 ld.add_action(start_robot_state_publisher_cmd)
286 ld.add_action(rviz_cmd)
287 ld.add_action(bringup_cmd)
288 ld.add_action(ekf_gps_localization)
289
290 return ld