|
1 | 1 | <?xml version="1.0"?>
|
2 | 2 | <launch>
|
| 3 | + <!-- only when running with a real vehicle, the pose_initializer judges the stop --> |
3 | 4 | <let name="stop_check_enabled" if="$(eval "'$(var system_run_mode)'=='online'")" value="true"/>
|
4 | 5 | <let name="stop_check_enabled" if="$(eval "'$(var system_run_mode)'=='logging_simulation'")" value="false"/>
|
5 | 6 |
|
| 7 | + <!-- When gnss_enabled is false, automatic_pose_initializer will not run, only manual initial position estimation is available. --> |
| 8 | + <arg name="gnss_enabled" default="true" description="gnss availability for initial position estimation"/> |
| 9 | + |
| 10 | + <!-- split string with underscores --> |
| 11 | + <let name="available_args" value="[\'ndt\',\'yabloc\',\'eagleye\',\'artag\']"/> |
| 12 | + <let name="split_function" value="list(set('$(var pose_source)'.split('_')).intersection($(var available_args)))"/> |
| 13 | + <let name="pose_sources" value="$(eval $(var split_function))"/> |
| 14 | + <let name="multi_localizer_mode" value="$(eval "len($(var pose_sources))> 1")"/> |
| 15 | + |
| 16 | + <!-- organizes flags for which nodes to activate --> |
| 17 | + <let name="use_ndt_pose" value="$(eval "'ndt' in $(var pose_sources)")"/> |
| 18 | + <let name="use_yabloc_pose" value="$(eval "'yabloc' in $(var pose_sources)")"/> |
| 19 | + <let name="use_artag_pose" value="$(eval "'artag' in $(var pose_sources)")"/> |
| 20 | + <let name="use_eagleye_pose" value="$(eval "'eagleye' in $(var pose_sources)")"/> |
| 21 | + <let name="use_eagleye_twist" value="$(eval "'eagleye' == '$(var twist_source)'")"/> |
| 22 | + <let name="use_gyro_odom_twist" value="$(eval "'gyro_odom' == '$(var twist_source)'")"/> |
| 23 | + |
6 | 24 | <!-- NDT Scan Matcher Launch (as pose estimator) -->
|
7 |
| - <group if="$(eval "'$(var pose_source)'=='ndt'")"> |
8 |
| - <group> |
9 |
| - <push-ros-namespace namespace="pose_estimator"/> |
10 |
| - <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml"/> |
11 |
| - </group> |
12 |
| - <group> |
13 |
| - <push-ros-namespace namespace="util"/> |
14 |
| - <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml"> |
15 |
| - <arg name="ndt_enabled" value="true"/> |
16 |
| - <arg name="gnss_enabled" value="true"/> |
17 |
| - <arg name="ekf_enabled" value="true"/> |
18 |
| - <arg name="yabloc_enabled" value="false"/> |
19 |
| - <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/> |
20 |
| - <arg name="config_file" value="$(var pose_initializer_param_path)"/> |
21 |
| - <arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/> |
22 |
| - </include> |
23 |
| - <group if="$(var gnss_enabled)"> |
24 |
| - <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/> |
25 |
| - </group> |
26 |
| - <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py"> |
27 |
| - <arg name="lidar_container_name" value="$(var lidar_container_name)"/> |
28 |
| - </include> |
29 |
| - </group> |
| 25 | + <group if="$(var use_ndt_pose)"> |
| 26 | + <push-ros-namespace namespace="pose_estimator"/> |
| 27 | + <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml"/> |
30 | 28 | </group>
|
31 | 29 |
|
32 | 30 | <!-- YabLoc Launch (as pose estimator) -->
|
33 |
| - <group if="$(eval "'$(var pose_source)'=='yabloc'")"> |
34 |
| - <group> |
35 |
| - <push-ros-namespace namespace="pose_estimator"/> |
36 |
| - <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/yabloc.launch.xml"/> |
37 |
| - </group> |
38 |
| - <group> |
39 |
| - <push-ros-namespace namespace="util"/> |
40 |
| - <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml"> |
41 |
| - <arg name="ndt_enabled" value="false"/> |
42 |
| - <arg name="gnss_enabled" value="true"/> |
43 |
| - <arg name="ekf_enabled" value="true"/> |
44 |
| - <arg name="yabloc_enabled" value="true"/> |
45 |
| - <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/> |
46 |
| - <arg name="config_file" value="$(var pose_initializer_param_path)"/> |
47 |
| - <arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/> |
48 |
| - </include> |
49 |
| - <group if="$(var gnss_enabled)"> |
50 |
| - <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/> |
51 |
| - </group> |
52 |
| - </group> |
| 31 | + <group if="$(var use_yabloc_pose)"> |
| 32 | + <push-ros-namespace namespace="pose_estimator"/> |
| 33 | + <let name="yabloc_src_image" value="/sensing/camera/traffic_light/image_raw" unless="$(var multi_localizer_mode)"/> |
| 34 | + <let name="yabloc_src_image" value="/sensing/camera/traffic_light/image_raw/yabloc_relay" if="$(var multi_localizer_mode)"/> |
| 35 | + |
| 36 | + <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/yabloc.launch.xml"> |
| 37 | + <arg name="src_image" value="$(var yabloc_src_image)"/> |
| 38 | + </include> |
53 | 39 | </group>
|
54 | 40 |
|
55 |
| - <!-- Gyro Odometer Launch (as pose estimator) --> |
56 |
| - <group if="$(eval "'$(var twist_source)'=='gyro_odom'")"> |
| 41 | + <!-- Gyro Odometer Launch (as twist estimator) --> |
| 42 | + <group if="$(var use_gyro_odom_twist)"> |
57 | 43 | <push-ros-namespace namespace="twist_estimator"/>
|
58 | 44 | <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/gyro_odometer.launch.xml"/>
|
59 | 45 | </group>
|
60 | 46 |
|
61 |
| - <!-- Eagleye Launch (as pose & twist estimator) --> |
62 |
| - <group if="$(eval "'$(var pose_source)'=='eagleye' and '$(var twist_source)'=='eagleye'")"> |
63 |
| - <group> |
64 |
| - <push-ros-namespace namespace="pose_twist_estimator"/> |
65 |
| - <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml"> |
66 |
| - <arg name="output_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/> |
67 |
| - <arg name="output_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/> |
68 |
| - <arg name="use_eagleye_pose" value="true"/> |
69 |
| - <arg name="use_eagleye_twist" value="true"/> |
70 |
| - </include> |
71 |
| - </group> |
72 |
| - <group> |
73 |
| - <push-ros-namespace namespace="util"/> |
74 |
| - <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml"> |
75 |
| - <arg name="ndt_enabled" value="false"/> |
76 |
| - <arg name="gnss_enabled" value="true"/> |
77 |
| - <arg name="ekf_enabled" value="true"/> |
78 |
| - <arg name="yabloc_enabled" value="false"/> |
79 |
| - <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/> |
80 |
| - <arg name="config_file" value="$(var pose_initializer_param_path)"/> |
81 |
| - <arg name="sub_gnss_pose_cov" value="/localization/pose_estimator/pose_with_covariance"/> |
82 |
| - </include> |
83 |
| - <group if="$(var gnss_enabled)"> |
84 |
| - <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/> |
85 |
| - </group> |
86 |
| - </group> |
87 |
| - </group> |
| 47 | + <!-- Eagleye Launch --> |
| 48 | + <group if="$(eval "$(var use_eagleye_pose) or $(var use_eagleye_twist)")"> |
| 49 | + <let name="eagleye_name_space" value="pose_twist_estimator"/> |
| 50 | + <let name="eagleye_name_space" value="twist_estimator" unless="$(var use_eagleye_pose)"/> |
| 51 | + <let name="eagleye_name_space" value="pose_estimator" unless="$(var use_eagleye_twist)"/> |
| 52 | + <let name="output_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance" unless="$(var multi_localizer_mode)"/> |
| 53 | + <let name="output_pose_with_cov_name" value="/localization/pose_estimator/eagleye/pose_with_covariance/to_relay" if="$(var multi_localizer_mode)"/> |
88 | 54 |
|
89 |
| - <!-- Eagleye Launch (as pose estimator) --> |
90 |
| - <group if="$(eval "'$(var pose_source)'=='eagleye' and '$(var twist_source)'!='eagleye'")"> |
91 |
| - <group> |
92 |
| - <push-ros-namespace namespace="pose_estimator"/> |
93 |
| - <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml"> |
94 |
| - <arg name="output_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/> |
95 |
| - <arg name="use_eagleye_pose" value="true"/> |
96 |
| - <arg name="use_eagleye_twist" value="false"/> |
97 |
| - </include> |
98 |
| - </group> |
99 |
| - <group> |
100 |
| - <push-ros-namespace namespace="util"/> |
101 |
| - <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml"> |
102 |
| - <arg name="ndt_enabled" value="false"/> |
103 |
| - <arg name="gnss_enabled" value="true"/> |
104 |
| - <arg name="ekf_enabled" value="true"/> |
105 |
| - <arg name="yabloc_enabled" value="false"/> |
106 |
| - <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/> |
107 |
| - <arg name="config_file" value="$(var pose_initializer_param_path)"/> |
108 |
| - <arg name="sub_gnss_pose_cov" value="/localization/pose_estimator/pose_with_covariance"/> |
109 |
| - </include> |
110 |
| - <group if="$(var gnss_enabled)"> |
111 |
| - <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/> |
112 |
| - </group> |
113 |
| - </group> |
114 |
| - </group> |
115 |
| - |
116 |
| - <!-- Eagleye Launch (as twist estimator) --> |
117 |
| - <group if="$(eval "'$(var pose_source)'!='eagleye' and '$(var twist_source)'=='eagleye'")"> |
118 |
| - <push-ros-namespace namespace="twist_estimator"/> |
| 55 | + <push-ros-namespace namespace="$(var eagleye_name_space)"/> |
119 | 56 | <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml">
|
120 | 57 | <arg name="output_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
|
121 |
| - <arg name="use_eagleye_pose" value="false"/> |
122 |
| - <arg name="use_eagleye_twist" value="true"/> |
| 58 | + <arg name="output_pose_with_cov_name" value="$(var output_pose_with_cov_name)"/> |
| 59 | + <arg name="use_eagleye_pose" value="$(var use_eagleye_pose)"/> |
| 60 | + <arg name="use_eagleye_twist" value="$(var use_eagleye_twist)"/> |
123 | 61 | </include>
|
124 | 62 | </group>
|
125 | 63 |
|
126 | 64 | <!-- AR Tag Based Localizer (as pose estimator) -->
|
127 |
| - <group if="$(eval "'$(var pose_source)'=='artag'")"> |
128 |
| - <group> |
129 |
| - <push-ros-namespace namespace="pose_estimator"/> |
130 |
| - <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml"/> |
| 65 | + <group if="$(var use_artag_pose)"> |
| 66 | + <push-ros-namespace namespace="pose_estimator"/> |
| 67 | + <let name="artag_input_image" value="/sensing/camera/traffic_light/image_raw" unless="$(var multi_localizer_mode)"/> |
| 68 | + <let name="artag_input_image" value="/sensing/camera/traffic_light/image_raw/artag_relay" if="$(var multi_localizer_mode)"/> |
| 69 | + |
| 70 | + <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml"> |
| 71 | + <arg name="input_image" value="$(var artag_input_image)"/> |
| 72 | + </include> |
| 73 | + </group> |
| 74 | + |
| 75 | + <!-- Pose Estimator Arbiter Launch --> |
| 76 | + <group if="$(var multi_localizer_mode)"> |
| 77 | + <include file="$(find-pkg-share pose_estimator_arbiter)/launch/pose_estimator_arbiter.launch.xml"> |
| 78 | + <arg name="pose_sources" value="$(var pose_sources)"/> |
| 79 | + <arg name="input_pointcloud" value="$(var input_pointcloud)"/> |
| 80 | + </include> |
| 81 | + </group> |
| 82 | + |
| 83 | + <!-- Util Launch --> |
| 84 | + <group> |
| 85 | + <push-ros-namespace namespace="util"/> |
| 86 | + |
| 87 | + <!-- pose_initializer --> |
| 88 | + <let name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/> |
| 89 | + <group if="$(var use_eagleye_pose)" scoped="false"> |
| 90 | + <let name="sub_gnss_pose_cov" value="/localization/pose_estimator/pose_with_covariance" unless="$(var multi_localizer_mode)"/> |
131 | 91 | </group>
|
132 | 92 |
|
133 |
| - <group> |
134 |
| - <push-ros-namespace namespace="util"/> |
135 |
| - <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml"> |
136 |
| - <arg name="ndt_enabled" value="false"/> |
137 |
| - <arg name="gnss_enabled" value="false"/> |
138 |
| - <arg name="ekf_enabled" value="true"/> |
139 |
| - <arg name="yabloc_enabled" value="false"/> |
140 |
| - <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/> |
141 |
| - <arg name="config_file" value="$(var pose_initializer_param_path)"/> |
142 |
| - <arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/> |
143 |
| - </include> |
144 |
| - <group if="$(var gnss_enabled)"> |
145 |
| - <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/> |
146 |
| - </group> |
147 |
| - <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py"> |
148 |
| - <arg name="lidar_container_name" value="$(var lidar_container_name)"/> |
149 |
| - </include> |
| 93 | + <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml"> |
| 94 | + <arg name="ndt_enabled" value="$(var use_ndt_pose)"/> |
| 95 | + <arg name="yabloc_enabled" value="$(var use_yabloc_pose)"/> |
| 96 | + <arg name="gnss_enabled" value="$(var gnss_enabled)"/> |
| 97 | + <arg name="ekf_enabled" value="true"/> |
| 98 | + <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/> |
| 99 | + <arg name="config_file" value="$(var pose_initializer_param_path)"/> |
| 100 | + <arg name="sub_gnss_pose_cov" value="$(var sub_gnss_pose_cov)"/> |
| 101 | + </include> |
| 102 | + |
| 103 | + <!-- automatic_pose_initializer --> |
| 104 | + <group if="$(var gnss_enabled)"> |
| 105 | + <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/> |
150 | 106 | </group>
|
| 107 | + |
| 108 | + <!-- pointcloud_downsampling --> |
| 109 | + <let name="override_input_pointcloud" value="$(var input_pointcloud)"/> |
| 110 | + <let name="override_input_pointcloud" value="$(var input_pointcloud)/relay" if="$(var multi_localizer_mode)"/> |
| 111 | + <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py" if="$(var use_ndt_pose)"> |
| 112 | + <arg name="lidar_container_name" value="$(var lidar_container_name)"/> |
| 113 | + <arg name="input_pointcloud" value="$(var override_input_pointcloud)"/> |
| 114 | + </include> |
151 | 115 | </group>
|
152 | 116 | </launch>
|
0 commit comments