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