Skip to content

Commit

Permalink
feat(pose_estimator_arbiter): add pose_estimator_arbiter with simple …
Browse files Browse the repository at this point in the history 
…switching rule (autowarefoundation#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>
  • Loading branch information
@KYabuuchi @pre-commit-ci
KYabuuchi and pre-commit-ci[bot] authored Feb 6, 2024
1 parent 9601a3e commit d4441bd
Show file tree
Hide file tree
Showing 41 changed files with 5,508 additions and 128 deletions.
4 changes: 2 additions & 2 deletions launch/tier4_localization_launch/launch/localization.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
<?xml version="1.0"?>
<launch>
<!-- Parameter files -->
<arg name="pose_source"/>
<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"/>
<arg name="twist_source"/>
<arg name="system_run_mode"/>

<!-- Parameter files -->
<arg name="crop_box_filter_measurement_range_param_path"/>
<arg name="voxel_grid_downsample_filter_param_path"/>
<arg name="random_downsample_filter_param_path"/>
Expand Down
4 changes: 3 additions & 1 deletion ...h/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,11 @@
<?xml version="1.0"?>
<launch>
<group>
<arg name="input_image"/>

<include file="$(find-pkg-share ar_tag_based_localizer)/launch/ar_tag_based_localizer.launch.xml">
<arg name="input_lanelet2_map" value="/map/vector_map"/>
<arg name="input_image" value="/sensing/camera/traffic_light/image_raw"/>
<arg name="input_image" value="$(var input_image)"/>
<arg name="input_camera_info" value="/sensing/camera/traffic_light/camera_info"/>
<arg name="input_ekf_pose" value="/localization/pose_twist_fusion_filter/biased_pose_with_covariance"/>
<arg name="output_pose_with_covariance" value="/localization/pose_estimator/pose_with_covariance"/>
Expand Down
214 changes: 89 additions & 125 deletions launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -1,152 +1,116 @@
<?xml version="1.0"?>
<launch>
<!-- only when running with a real vehicle, the pose_initializer judges the stop -->
<let name="stop_check_enabled" if="$(eval &quot;'$(var system_run_mode)'=='online'&quot;)" value="true"/>
<let name="stop_check_enabled" if="$(eval &quot;'$(var system_run_mode)'=='logging_simulation'&quot;)" value="false"/>

<!-- When gnss_enabled is false, automatic_pose_initializer will not run, only manual initial position estimation is available. -->
<arg name="gnss_enabled" default="true" description="gnss availability for initial position estimation"/>

<!-- split string with underscores -->
<let name="available_args" value="[\'ndt\',\'yabloc\',\'eagleye\',\'artag\']"/>
<let name="split_function" value="list(set('$(var pose_source)'.split('_')).intersection($(var available_args)))"/>
<let name="pose_sources" value="$(eval $(var split_function))"/>
<let name="multi_localizer_mode" value="$(eval &quot;len($(var pose_sources))> 1&quot;)"/>

<!-- organizes flags for which nodes to activate -->
<let name="use_ndt_pose" value="$(eval &quot;'ndt' in $(var pose_sources)&quot;)"/>
<let name="use_yabloc_pose" value="$(eval &quot;'yabloc' in $(var pose_sources)&quot;)"/>
<let name="use_artag_pose" value="$(eval &quot;'artag' in $(var pose_sources)&quot;)"/>
<let name="use_eagleye_pose" value="$(eval &quot;'eagleye' in $(var pose_sources)&quot;)"/>
<let name="use_eagleye_twist" value="$(eval &quot;'eagleye' == '$(var twist_source)'&quot;)"/>
<let name="use_gyro_odom_twist" value="$(eval &quot;'gyro_odom' == '$(var twist_source)'&quot;)"/>

<!-- NDT Scan Matcher Launch (as pose estimator) -->
<group if="$(eval &quot;'$(var pose_source)'=='ndt'&quot;)">
<group>
<push-ros-namespace namespace="pose_estimator"/>
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml"/>
</group>
<group>
<push-ros-namespace namespace="util"/>
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
<arg name="ndt_enabled" value="true"/>
<arg name="gnss_enabled" value="true"/>
<arg name="ekf_enabled" value="true"/>
<arg name="yabloc_enabled" value="false"/>
<arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
<arg name="config_file" value="$(var pose_initializer_param_path)"/>
<arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/>
</include>
<group if="$(var gnss_enabled)">
<include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
</group>
<include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
<arg name="lidar_container_name" value="$(var lidar_container_name)"/>
</include>
</group>
<group if="$(var use_ndt_pose)">
<push-ros-namespace namespace="pose_estimator"/>
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml"/>
</group>

<!-- YabLoc Launch (as pose estimator) -->
<group if="$(eval &quot;'$(var pose_source)'=='yabloc'&quot;)">
<group>
<push-ros-namespace namespace="pose_estimator"/>
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/yabloc.launch.xml"/>
</group>
<group>
<push-ros-namespace namespace="util"/>
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
<arg name="ndt_enabled" value="false"/>
<arg name="gnss_enabled" value="true"/>
<arg name="ekf_enabled" value="true"/>
<arg name="yabloc_enabled" value="true"/>
<arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
<arg name="config_file" value="$(var pose_initializer_param_path)"/>
<arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/>
</include>
<group if="$(var gnss_enabled)">
<include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
</group>
</group>
<group if="$(var use_yabloc_pose)">
<push-ros-namespace namespace="pose_estimator"/>
<let name="yabloc_src_image" value="/sensing/camera/traffic_light/image_raw" unless="$(var multi_localizer_mode)"/>
<let name="yabloc_src_image" value="/sensing/camera/traffic_light/image_raw/yabloc_relay" if="$(var multi_localizer_mode)"/>

<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/yabloc.launch.xml">
<arg name="src_image" value="$(var yabloc_src_image)"/>
</include>
</group>

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

<!-- Eagleye Launch (as pose & twist estimator) -->
<group if="$(eval &quot;'$(var pose_source)'=='eagleye' and '$(var twist_source)'=='eagleye'&quot;)">
<group>
<push-ros-namespace namespace="pose_twist_estimator"/>
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml">
<arg name="output_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
<arg name="output_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
<arg name="use_eagleye_pose" value="true"/>
<arg name="use_eagleye_twist" value="true"/>
</include>
</group>
<group>
<push-ros-namespace namespace="util"/>
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
<arg name="ndt_enabled" value="false"/>
<arg name="gnss_enabled" value="true"/>
<arg name="ekf_enabled" value="true"/>
<arg name="yabloc_enabled" value="false"/>
<arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
<arg name="config_file" value="$(var pose_initializer_param_path)"/>
<arg name="sub_gnss_pose_cov" value="/localization/pose_estimator/pose_with_covariance"/>
</include>
<group if="$(var gnss_enabled)">
<include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
</group>
</group>
</group>
<!-- Eagleye Launch -->
<group if="$(eval &quot;$(var use_eagleye_pose) or $(var use_eagleye_twist)&quot;)">
<let name="eagleye_name_space" value="pose_twist_estimator"/>
<let name="eagleye_name_space" value="twist_estimator" unless="$(var use_eagleye_pose)"/>
<let name="eagleye_name_space" value="pose_estimator" unless="$(var use_eagleye_twist)"/>
<let name="output_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance" unless="$(var multi_localizer_mode)"/>
<let name="output_pose_with_cov_name" value="/localization/pose_estimator/eagleye/pose_with_covariance/to_relay" if="$(var multi_localizer_mode)"/>

<!-- Eagleye Launch (as pose estimator) -->
<group if="$(eval &quot;'$(var pose_source)'=='eagleye' and '$(var twist_source)'!='eagleye'&quot;)">
<group>
<push-ros-namespace namespace="pose_estimator"/>
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml">
<arg name="output_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
<arg name="use_eagleye_pose" value="true"/>
<arg name="use_eagleye_twist" value="false"/>
</include>
</group>
<group>
<push-ros-namespace namespace="util"/>
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
<arg name="ndt_enabled" value="false"/>
<arg name="gnss_enabled" value="true"/>
<arg name="ekf_enabled" value="true"/>
<arg name="yabloc_enabled" value="false"/>
<arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
<arg name="config_file" value="$(var pose_initializer_param_path)"/>
<arg name="sub_gnss_pose_cov" value="/localization/pose_estimator/pose_with_covariance"/>
</include>
<group if="$(var gnss_enabled)">
<include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
</group>
</group>
</group>

<!-- Eagleye Launch (as twist estimator) -->
<group if="$(eval &quot;'$(var pose_source)'!='eagleye' and '$(var twist_source)'=='eagleye'&quot;)">
<push-ros-namespace namespace="twist_estimator"/>
<push-ros-namespace namespace="$(var eagleye_name_space)"/>
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml">
<arg name="output_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
<arg name="use_eagleye_pose" value="false"/>
<arg name="use_eagleye_twist" value="true"/>
<arg name="output_pose_with_cov_name" value="$(var output_pose_with_cov_name)"/>
<arg name="use_eagleye_pose" value="$(var use_eagleye_pose)"/>
<arg name="use_eagleye_twist" value="$(var use_eagleye_twist)"/>
</include>
</group>

<!-- AR Tag Based Localizer (as pose estimator) -->
<group if="$(eval &quot;'$(var pose_source)'=='artag'&quot;)">
<group>
<push-ros-namespace namespace="pose_estimator"/>
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml"/>
<group if="$(var use_artag_pose)">
<push-ros-namespace namespace="pose_estimator"/>
<let name="artag_input_image" value="/sensing/camera/traffic_light/image_raw" unless="$(var multi_localizer_mode)"/>
<let name="artag_input_image" value="/sensing/camera/traffic_light/image_raw/artag_relay" if="$(var multi_localizer_mode)"/>

<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml">
<arg name="input_image" value="$(var artag_input_image)"/>
</include>
</group>

<!-- Pose Estimator Arbiter Launch -->
<group if="$(var multi_localizer_mode)">
<include file="$(find-pkg-share pose_estimator_arbiter)/launch/pose_estimator_arbiter.launch.xml">
<arg name="pose_sources" value="$(var pose_sources)"/>
<arg name="input_pointcloud" value="$(var input_pointcloud)"/>
</include>
</group>

<!-- Util Launch -->
<group>
<push-ros-namespace namespace="util"/>

<!-- pose_initializer -->
<let name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/>
<group if="$(var use_eagleye_pose)" scoped="false">
<let name="sub_gnss_pose_cov" value="/localization/pose_estimator/pose_with_covariance" unless="$(var multi_localizer_mode)"/>
</group>

<group>
<push-ros-namespace namespace="util"/>
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
<arg name="ndt_enabled" value="false"/>
<arg name="gnss_enabled" value="false"/>
<arg name="ekf_enabled" value="true"/>
<arg name="yabloc_enabled" value="false"/>
<arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
<arg name="config_file" value="$(var pose_initializer_param_path)"/>
<arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/>
</include>
<group if="$(var gnss_enabled)">
<include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
</group>
<include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
<arg name="lidar_container_name" value="$(var lidar_container_name)"/>
</include>
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
<arg name="ndt_enabled" value="$(var use_ndt_pose)"/>
<arg name="yabloc_enabled" value="$(var use_yabloc_pose)"/>
<arg name="gnss_enabled" value="$(var gnss_enabled)"/>
<arg name="ekf_enabled" value="true"/>
<arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
<arg name="config_file" value="$(var pose_initializer_param_path)"/>
<arg name="sub_gnss_pose_cov" value="$(var sub_gnss_pose_cov)"/>
</include>

<!-- automatic_pose_initializer -->
<group if="$(var gnss_enabled)">
<include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
</group>

<!-- pointcloud_downsampling -->
<let name="override_input_pointcloud" value="$(var input_pointcloud)"/>
<let name="override_input_pointcloud" value="$(var input_pointcloud)/relay" if="$(var multi_localizer_mode)"/>
<include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py" if="$(var use_ndt_pose)">
<arg name="lidar_container_name" value="$(var lidar_container_name)"/>
<arg name="input_pointcloud" value="$(var override_input_pointcloud)"/>
</include>
</group>
</launch>
1 change: 1 addition & 0 deletions launch/tier4_localization_launch/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
<exec_depend>gyro_odometer</exec_depend>
<exec_depend>ndt_scan_matcher</exec_depend>
<exec_depend>pointcloud_preprocessor</exec_depend>
<exec_depend>pose_estimator_arbiter</exec_depend>
<exec_depend>pose_initializer</exec_depend>
<exec_depend>pose_instability_detector</exec_depend>
<exec_depend>topic_tools</exec_depend>
Expand Down
58 changes: 58 additions & 0 deletions localization/pose_estimator_arbiter/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
cmake_minimum_required(VERSION 3.14)
project(pose_estimator_arbiter)

find_package(autoware_cmake REQUIRED)
autoware_package()

find_package(glog REQUIRED)

find_package(PCL REQUIRED COMPONENTS common)
include_directories(SYSTEM ${PCL_INCLUDE_DIRS})

# ==============================
# switch rule library
ament_auto_add_library(switch_rule
SHARED
src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp
)
target_include_directories(switch_rule PUBLIC src)

# ==============================
# pose estimator arbiter node
ament_auto_add_executable(${PROJECT_NAME}
src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp
src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp
)
target_include_directories(${PROJECT_NAME} PUBLIC src)
target_link_libraries(${PROJECT_NAME} switch_rule glog::glog)

# ==============================
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)

# define test definition macro
function(add_testcase filepath)
get_filename_component(filename ${filepath} NAME)
string(REGEX REPLACE ".cpp" "" test_name ${filename})
ament_add_gmock(${test_name} ${filepath})
target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src)
target_link_libraries(${test_name} fmt)
ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
endfunction()

add_testcase(test/test_shared_data.cpp)
add_ros_test(
test/test_pose_estimator_arbiter.py
TIMEOUT "30"
)
endif()

# ==============================
# In practice, the example rule is not used as autoware code.
# It exists only for user reference and is tested only.
add_subdirectory(example_rule)

# ==============================
ament_auto_package(INSTALL_TO_SHARE
launch
)
Loading

0 comments on commit d4441bd

Please sign in to comment.