Skip to content

Commit d4441bd

Browse files
KYabuuchipre-commit-ci[bot]
KYabuuchi
and
pre-commit-ci[bot]
authored
feat(pose_estimator_arbiter): add pose_estimator_arbiter with simple switching rule (#6144)
* implement pose_estimator_manager pkg Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * tmp Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * swap ndt & yabloc Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add suspension service in yabloc particle filter Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add pluginlib for switching rule Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * implement switch rule using pluginlib Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * WIP: implement simple_switch_rule Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * implement pcd_occupancy_rule based switcher Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * resolve conflicts occured by rebase Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * sub_manager_node is not necessary Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add ndt_yabloc_eagleye Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix bug Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * intuitive_multi_pose_estimator_launch Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * yabloc_pf shoulbe be activated at the first Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * merge swith_rule_plugin as inheritance Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix launch bug Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add eagleye_area rule Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * implement strict switching rule Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * refine message Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix merge conflict Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * use hysteresis threshold for pcd occupancy criteria Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix merge conflict Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add gtest Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add component test Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add artag submanager Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add ar_tag_position to get ar-tag position Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * check distance to nearest ar marker Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * switch ARTAG localizer if ar marker locates around ego Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * improve ar_tag_position.[hc]pp Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * split update() from map_base_rule.cpp Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * apply pre-commit Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add license description Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * update include guard Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * reflected all pre-commit's points Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * use magic_enum Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add pcd_occupancy helper Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * change directory structure Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * change namespace Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * remap some topics Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * update test Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add shared_data to share data Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * remove obsolete comments Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * share subscribed data by SharedData Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * remove obsolete comments and fix to pass test.py Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * rename SharedData Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * stream debug_msg as is Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add README.md Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * Update README.md update README.md on github Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix eagleye bug Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * update README Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * wip Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * update README.md Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * update README Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * use landmark_manager Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add glog & fix rule_helper bug Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * publish empty diagnostics Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix artag arbitorator Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * implement callback_involving_variable Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * rename invokingVariable Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * clarify log level Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * update diagnostics Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * adope new landmark_manager Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * rename manager arbiter Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * style(pre-commit): autofix * fix obsolete change Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * change yabloc relayed input topic Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * resolve merge conflict Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * adopt ar_tag_position for new ar tag map specification Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * rename sub_arbitr to stopper Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * apply pre-commit Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add timeout for async parameter client Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * style(pre-commit): autofix * fix typo Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * refactor shared_data Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * rename yabloc_suspend_service Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * improve debug log Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix integration test Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * style(pre-commit): autofix * remove obsolete notation Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix ar_tag_based_localizer.launch.xml Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * again fix ar_tag_based_localizer.launch.xml Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * style(pre-commit): autofix * add sample data url Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * (review reflect) refactor launch Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * (review reflect) refactor launch about gnss_enabled Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * (review reflect) organize type alias accessibility Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * (review reflect) rename PoseEstimatorName to PoseEstimatorType Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * (review reflect) fix typo Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * style(pre-commit): autofix * fix pedantic warning of PCL Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * (review reflect) improve diag & suppress warning Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * (review reflect) create sub only when the corresponding estimator is running Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * rename eagleye_area to pose_estimator_area Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * vectormap based rule works well Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * move old rules to example_rule/ Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * update README Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * improve some features Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * style(pre-commit): autofix * style(pre-commit): autofix * move some rules into example_rule & add new simple rule Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * apply pre-commit & update README Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * split CMake for example_rule Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * remove ar_tag_position & simplify example switching rule Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add vector_map_based_rule test Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add pcd_map_based_rule test Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * improve README Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix integration test.py Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add test Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * refactor & update README Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * replace obsolete video Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix typo Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * Update README.md fix markdown (add one line just after <summary>) Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * use structures bindings Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * add many comments Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * remove obsolete include & alias Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix miss of eagleye output relay Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * fix 404 URL Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * remove obsolete args Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> --------- Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 9601a3e commit d4441bd

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

41 files changed

+5508
-128
lines changed

‎launch/tier4_localization_launch/launch/localization.launch.xml

+2-2
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,10 @@
11
<?xml version="1.0"?>
22
<launch>
3-
<!-- Parameter files -->
4-
<arg name="pose_source"/>
3+
<arg name="pose_source" description="A string consisting of ndt, yabloc, artag and eagleye joined by underscores no matter the order. e.g. ndt_yabloc"/>
54
<arg name="twist_source"/>
65
<arg name="system_run_mode"/>
76

7+
<!-- Parameter files -->
88
<arg name="crop_box_filter_measurement_range_param_path"/>
99
<arg name="voxel_grid_downsample_filter_param_path"/>
1010
<arg name="random_downsample_filter_param_path"/>

‎launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml

+3-1
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,11 @@
11
<?xml version="1.0"?>
22
<launch>
33
<group>
4+
<arg name="input_image"/>
5+
46
<include file="$(find-pkg-share ar_tag_based_localizer)/launch/ar_tag_based_localizer.launch.xml">
57
<arg name="input_lanelet2_map" value="/map/vector_map"/>
6-
<arg name="input_image" value="/sensing/camera/traffic_light/image_raw"/>
8+
<arg name="input_image" value="$(var input_image)"/>
79
<arg name="input_camera_info" value="/sensing/camera/traffic_light/camera_info"/>
810
<arg name="input_ekf_pose" value="/localization/pose_twist_fusion_filter/biased_pose_with_covariance"/>
911
<arg name="output_pose_with_covariance" value="/localization/pose_estimator/pose_with_covariance"/>

‎launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

+89-125
Original file line numberDiff line numberDiff line change
@@ -1,152 +1,116 @@
11
<?xml version="1.0"?>
22
<launch>
3+
<!-- only when running with a real vehicle, the pose_initializer judges the stop -->
34
<let name="stop_check_enabled" if="$(eval &quot;'$(var system_run_mode)'=='online'&quot;)" value="true"/>
45
<let name="stop_check_enabled" if="$(eval &quot;'$(var system_run_mode)'=='logging_simulation'&quot;)" value="false"/>
56

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 &quot;len($(var pose_sources))> 1&quot;)"/>
15+
16+
<!-- organizes flags for which nodes to activate -->
17+
<let name="use_ndt_pose" value="$(eval &quot;'ndt' in $(var pose_sources)&quot;)"/>
18+
<let name="use_yabloc_pose" value="$(eval &quot;'yabloc' in $(var pose_sources)&quot;)"/>
19+
<let name="use_artag_pose" value="$(eval &quot;'artag' in $(var pose_sources)&quot;)"/>
20+
<let name="use_eagleye_pose" value="$(eval &quot;'eagleye' in $(var pose_sources)&quot;)"/>
21+
<let name="use_eagleye_twist" value="$(eval &quot;'eagleye' == '$(var twist_source)'&quot;)"/>
22+
<let name="use_gyro_odom_twist" value="$(eval &quot;'gyro_odom' == '$(var twist_source)'&quot;)"/>
23+
624
<!-- NDT Scan Matcher Launch (as pose estimator) -->
7-
<group if="$(eval &quot;'$(var pose_source)'=='ndt'&quot;)">
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"/>
3028
</group>
3129

3230
<!-- YabLoc Launch (as pose estimator) -->
33-
<group if="$(eval &quot;'$(var pose_source)'=='yabloc'&quot;)">
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>
5339
</group>
5440

55-
<!-- Gyro Odometer Launch (as pose estimator) -->
56-
<group if="$(eval &quot;'$(var twist_source)'=='gyro_odom'&quot;)">
41+
<!-- Gyro Odometer Launch (as twist estimator) -->
42+
<group if="$(var use_gyro_odom_twist)">
5743
<push-ros-namespace namespace="twist_estimator"/>
5844
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/gyro_odometer.launch.xml"/>
5945
</group>
6046

61-
<!-- Eagleye Launch (as pose & twist estimator) -->
62-
<group if="$(eval &quot;'$(var pose_source)'=='eagleye' and '$(var twist_source)'=='eagleye'&quot;)">
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 &quot;$(var use_eagleye_pose) or $(var use_eagleye_twist)&quot;)">
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)"/>
8854

89-
<!-- Eagleye Launch (as pose estimator) -->
90-
<group if="$(eval &quot;'$(var pose_source)'=='eagleye' and '$(var twist_source)'!='eagleye'&quot;)">
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 &quot;'$(var pose_source)'!='eagleye' and '$(var twist_source)'=='eagleye'&quot;)">
118-
<push-ros-namespace namespace="twist_estimator"/>
55+
<push-ros-namespace namespace="$(var eagleye_name_space)"/>
11956
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml">
12057
<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)"/>
12361
</include>
12462
</group>
12563

12664
<!-- AR Tag Based Localizer (as pose estimator) -->
127-
<group if="$(eval &quot;'$(var pose_source)'=='artag'&quot;)">
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)"/>
13191
</group>
13292

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"/>
150106
</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>
151115
</group>
152116
</launch>

‎launch/tier4_localization_launch/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
<exec_depend>gyro_odometer</exec_depend>
2828
<exec_depend>ndt_scan_matcher</exec_depend>
2929
<exec_depend>pointcloud_preprocessor</exec_depend>
30+
<exec_depend>pose_estimator_arbiter</exec_depend>
3031
<exec_depend>pose_initializer</exec_depend>
3132
<exec_depend>pose_instability_detector</exec_depend>
3233
<exec_depend>topic_tools</exec_depend>

‎localization/pose_estimator_arbiter/CMakeLists.txt

+58
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(pose_estimator_arbiter)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
find_package(glog REQUIRED)
8+
9+
find_package(PCL REQUIRED COMPONENTS common)
10+
include_directories(SYSTEM ${PCL_INCLUDE_DIRS})
11+
12+
# ==============================
13+
# switch rule library
14+
ament_auto_add_library(switch_rule
15+
SHARED
16+
src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp
17+
)
18+
target_include_directories(switch_rule PUBLIC src)
19+
20+
# ==============================
21+
# pose estimator arbiter node
22+
ament_auto_add_executable(${PROJECT_NAME}
23+
src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp
24+
src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp
25+
)
26+
target_include_directories(${PROJECT_NAME} PUBLIC src)
27+
target_link_libraries(${PROJECT_NAME} switch_rule glog::glog)
28+
29+
# ==============================
30+
if(BUILD_TESTING)
31+
find_package(ament_cmake_gtest REQUIRED)
32+
33+
# define test definition macro
34+
function(add_testcase filepath)
35+
get_filename_component(filename ${filepath} NAME)
36+
string(REGEX REPLACE ".cpp" "" test_name ${filename})
37+
ament_add_gmock(${test_name} ${filepath})
38+
target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src)
39+
target_link_libraries(${test_name} fmt)
40+
ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
41+
endfunction()
42+
43+
add_testcase(test/test_shared_data.cpp)
44+
add_ros_test(
45+
test/test_pose_estimator_arbiter.py
46+
TIMEOUT "30"
47+
)
48+
endif()
49+
50+
# ==============================
51+
# In practice, the example rule is not used as autoware code.
52+
# It exists only for user reference and is tested only.
53+
add_subdirectory(example_rule)
54+
55+
# ==============================
56+
ament_auto_package(INSTALL_TO_SHARE
57+
launch
58+
)

0 commit comments

Comments
 (0)