From 6a2320e0709a898de64a83d1f44d5e4b73a75765 Mon Sep 17 00:00:00 2001
From: SakodaShintaro <rgbygscrsedppbwg@gmail.com>
Date: Tue, 13 Feb 2024 17:38:31 +0900
Subject: [PATCH] fix(localization): add ar tag based localizer param (#871)

Added ar_tag_based_localizer.param.yaml

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
---
 .../ar_tag_based_localizer.param.yaml         | 40 +++++++++++++++++++
 .../tier4_localization_component.launch.xml   |  3 ++
 2 files changed, 43 insertions(+)
 create mode 100644 autoware_launch/config/localization/ar_tag_based_localizer.param.yaml

diff --git a/autoware_launch/config/localization/ar_tag_based_localizer.param.yaml b/autoware_launch/config/localization/ar_tag_based_localizer.param.yaml
new file mode 100644
index 0000000000..e8f9016537
--- /dev/null
+++ b/autoware_launch/config/localization/ar_tag_based_localizer.param.yaml
@@ -0,0 +1,40 @@
+/**:
+  ros__parameters:
+    # marker_size
+    marker_size: 0.6
+
+    # target_tag_ids
+    target_tag_ids: ['0','1','2','3','4','5','6']
+
+    # base_covariance
+    #   This value is dynamically scaled according to the distance at which AR tags are detected.
+    base_covariance: [0.2, 0.0, 0.0, 0.0,  0.0,  0.0,
+                      0.0, 0.2, 0.0, 0.0,  0.0,  0.0,
+                      0.0, 0.0, 0.2, 0.0,  0.0,  0.0,
+                      0.0, 0.0, 0.0, 0.02, 0.0,  0.0,
+                      0.0, 0.0, 0.0, 0.0,  0.02, 0.0,
+                      0.0, 0.0, 0.0, 0.0,  0.0,  0.02]
+
+    # Detect AR-Tags within this range and publish the pose of ego vehicle
+    distance_threshold: 13.0  # [m]
+
+    # consider_orientation
+    consider_orientation: false
+
+    # Detector parameters
+    # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126
+    detection_mode: "DM_NORMAL"  # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST]
+    min_marker_size: 0.02
+
+    # Parameters for comparison with EKF Pose
+    # If the difference between the EKF pose and the current pose is within the range of values set below, the current pose is published.
+    # [How to determine the value]
+    # * ekf_time_tolerance: Since it is abnormal if the data comes too old from EKF, the tentative tolerance value is set at 5 seconds.
+    #                       This value is assumed to be unaffected even if it is increased or decreased by some amount.
+    # * ekf_position_tolerance: Since it is possible that multiple AR tags with the same ID could be placed, the tolerance should be as small as possible.
+    #                           And if the vehicle is running only on odometry in a section without AR tags,
+    #                           it is possible that self-position estimation could be off by a few meters.
+    #                           it should be fixed by AR tag detection, so tolerance should not be smaller than 10 meters.
+    #                           Therefore, the tolerance is set at 10 meters.
+    ekf_time_tolerance: 5.0 # [s]
+    ekf_position_tolerance: 10.0 # [m]
diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml
index c5117009d2..3ccd98104d 100644
--- a/autoware_launch/launch/components/tier4_localization_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml
@@ -52,6 +52,9 @@
 
       <!-- parameter paths for eagleye -->
       <arg name="eagleye_param_path" value="$(find-pkg-share autoware_launch)/config/localization/eagleye_config.param.yaml"/>
+
+      <!-- parameter path for ar_tag_based_localizer -->
+      <arg name="ar_tag_based_localizer_param_path" value="$(var loc_config_path)/ar_tag_based_localizer.param.yaml"/>
     </include>
   </group>
 </launch>