SMACC2
navigation_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
15import os
16
17from ament_index_python.packages import get_package_share_directory
18
19from launch import LaunchDescription
20from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
21from launch.substitutions import LaunchConfiguration
22from launch_ros.actions import Node
23from nav2_common.launch import RewrittenYaml
24
25
27 # Get the launch directory
28 sm_aws_warehouse_navigation_dir = get_package_share_directory("sm_aws_warehouse_navigation")
29 nav_configuration_dir = get_package_share_directory("nav2_bringup")
30
31 namespace = LaunchConfiguration("namespace")
32 use_sim_time = LaunchConfiguration("use_sim_time")
33 autostart = LaunchConfiguration("autostart")
34 params_file = LaunchConfiguration("params_file")
35 default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml")
36 # map_subscribe_transient_local = LaunchConfiguration("map_subscribe_transient_local")
37
38 params_file_path = os.path.join(
39 sm_aws_warehouse_navigation_dir,
40 "params",
41 "nav2z_client",
42 "nav2_params.yaml",
43 )
44
45 # params_file_path = os.path.join(
46 # nav_configuration_dir,
47 # "params",
48 # "nav2_params.yaml",
49 # )
50
51 default_nav_bt_tree_xml_filepath = os.path.join(
52 sm_aws_warehouse_navigation_dir, "params", "nav2z_client", "navigation_tree.xml"
53 )
54
55 # default_nav_bt_tree_xml_filepath = os.path.join(
56 # get_package_share_directory("nav2_bt_navigator"), "behavior_trees", "navigate_to_pose_w_replanning_and_recovery.xml")
57
58 # default_nav_bt_tree_xml_filepath = os.path.join(get_package_share_directory('nav2_bt_navigator'),
59 # 'behavior_trees', 'navigate_w_replanning_and_recovery.xml')
60
61 params_file_path = os.path.join(
62 sm_aws_warehouse_navigation_dir,
63 "params",
64 "nav2z_client",
65 "nav2_params.yaml",
66 )
67
68 # params_file_path = os.path.join(
69 # nav_configuration_dir,
70 # "params",
71 # "nav2_params.yaml",
72 # )
73
74 default_nav_bt_tree_xml_filepath = os.path.join(
75 sm_aws_warehouse_navigation_dir, "params", "nav2z_client", "navigation_tree.xml"
76 )
77
78 # default_nav_bt_tree_xml_filepath = os.path.join(
79 # get_package_share_directory("nav2_bt_navigator"), "behavior_trees", "navigate_to_pose_w_replanning_and_recovery.xml")
80
81 # default_nav_bt_tree_xml_filepath = os.path.join(get_package_share_directory('nav2_bt_navigator'),
82 # 'behavior_trees', 'navigate_w_replanning_and_recovery.xml')
83
84 lifecycle_nodes = ["controller_server", "planner_server", "recoveries_server", "bt_navigator"]
85
86 # Map fully qualified names to relative ones so the node's namespace can be prepended.
87 # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
88 # https://github.com/ros/geometry2/issues/32
89 # https://github.com/ros/robot_state_publisher/pull/30
90 # TODO(orduno) Substitute with `PushNodeRemapping`
91 # https://github.com/ros2/launch_ros/issues/56
92 remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
93
94 # Create our own temporary YAML files that include substitutions
95 param_substitutions = {
96 "use_sim_time": use_sim_time,
97 # 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml,
98 "default_nav_to_pose_bt_xml": default_nav_bt_tree_xml_filepath,
99 # "autostart": autostart,
100 # "map_subscribe_transient_local": map_subscribe_transient_local,
101 }
102
103 configured_params = RewrittenYaml(
104 source_file=params_file,
105 root_key=namespace,
106 param_rewrites=param_substitutions,
107 convert_types=True,
108 )
109
110 xtermprefix = (
111 "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' "
112 "-hold -geometry 1000x600 -sl 10000 -e"
113 )
114
115 print("+********************************")
116 print(str(param_substitutions))
117 print(str(default_nav_to_pose_bt_xml))
118
119 return LaunchDescription(
120 [
121 # Set env var to print messages to stdout immediately
122 SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"),
123 DeclareLaunchArgument(
124 "namespace", default_value="", description="Top-level namespace"
125 ),
126 DeclareLaunchArgument(
127 "use_sim_time",
128 default_value="false",
129 description="Use simulation (Gazebo) clock if true",
130 ),
131 DeclareLaunchArgument(
132 "autostart",
133 default_value="true",
134 description="Automatically startup the nav2 stack",
135 ),
136 DeclareLaunchArgument(
137 "params_file",
138 default_value=params_file_path,
139 description="Full path to the ROS2 parameters file to use",
140 ),
141 DeclareLaunchArgument(
142 "default_nav_to_pose_bt_xml",
143 # default_value=os.path.join(get_package_share_directory('nav2_bt_navigator'),
144 # 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
145 default_value=os.path.join(
146 nav_configuration_dir,
147 "params",
148 "nav2z_client",
149 "navigation_tree.xml",
150 ),
151 description="Full path to the behavior tree xml file to use",
152 ),
153 DeclareLaunchArgument(
154 "map_subscribe_transient_local",
155 default_value="false",
156 description="Whether to set the map subscriber QoS to transient local",
157 ),
158 Node(
159 package="nav2_controller",
160 executable="controller_server",
161 output="screen",
162 prefix=xtermprefix,
163 parameters=[configured_params],
164 remappings=remappings,
165 arguments=["--ros-args", "--log-level", "INFO"],
166 ),
167 Node(
168 package="nav2_planner",
169 executable="planner_server",
170 name="planner_server",
171 output="screen",
172 prefix=xtermprefix,
173 parameters=[configured_params],
174 remappings=remappings,
175 arguments=["--ros-args", "--log-level", "INFO"],
176 ),
177 Node(
178 package="nav2_recoveries",
179 executable="recoveries_server",
180 name="recoveries_server",
181 output="screen",
182 parameters=[configured_params],
183 remappings=remappings,
184 arguments=["--ros-args", "--log-level", "INFO"],
185 ),
186 Node(
187 package="nav2_bt_navigator",
188 executable="bt_navigator",
189 name="bt_navigator",
190 output="screen",
191 prefix=xtermprefix,
192 parameters=[configured_params],
193 remappings=remappings,
194 arguments=["--ros-args", "--log-level", "INFO"],
195 ),
196 Node(
197 package="nav2_waypoint_follower",
198 executable="waypoint_follower",
199 name="waypoint_follower",
200 output="screen",
201 parameters=[configured_params],
202 remappings=remappings,
203 ),
204 Node(
205 package="nav2_lifecycle_manager",
206 executable="lifecycle_manager",
207 name="lifecycle_manager_navigation",
208 output="screen",
209 prefix=xtermprefix,
210 parameters=[
211 {"use_sim_time": use_sim_time},
212 {"autostart": autostart},
213 {"node_names": lifecycle_nodes},
214 ],
215 arguments=["--ros-args", "--log-level", "INFO"],
216 ),
217 ]
218 )