From 0661239d0992694ff8e3da7ce76d29d43d71e679 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Thu, 7 Jul 2022 10:03:41 +0900
Subject: [PATCH 01/20] feat(stop_accel_evaluator): use flexible lpf

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../stop_accel_evaluator/src/stop_accel_evaluator_node.cpp    | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp b/control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp
index 0d553f80..03ceca05 100644
--- a/control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp
+++ b/control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp
@@ -45,8 +45,8 @@ StopAccelEvaluatorNode::StopAccelEvaluatorNode(const rclcpp::NodeOptions & node_
   stop_valid_imu_accel_num_ = this->declare_parameter("stop_valid_imu_accel_num", 4);
 
   {  // lowpass filter
-    lpf_pitch_ = std::make_shared<LowpassFilter1d>(0.0, this->declare_parameter("lpf_pitch_gain", 0.95));
-    lpf_acc_ = std::make_shared<LowpassFilter1d>(0.0, this->declare_parameter("lpf_acc_gain", 0.2));
+    lpf_pitch_ = std::make_shared<LowpassFilter1d>(this->declare_parameter("lpf_pitch_gain", 0.95));
+    lpf_acc_ = std::make_shared<LowpassFilter1d>(this->declare_parameter("lpf_acc_gain", 0.2));
   }
 
   sub_twist_ = this->create_subscription<Odometry>(

From b0ec9b747a5ee159bcc650cb52d521c6720646a1 Mon Sep 17 00:00:00 2001
From: yabuta <makoto.yabuta@tier4.jp>
Date: Thu, 7 Jul 2022 10:07:13 +0900
Subject: [PATCH 02/20] fix pr template

---
 .github/PULL_REQUEST_TEMPLATE.md | 61 +++++++++++++-------------------
 1 file changed, 24 insertions(+), 37 deletions(-)

diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md
index 66a6f11f..5a48d410 100644
--- a/.github/PULL_REQUEST_TEMPLATE.md
+++ b/.github/PULL_REQUEST_TEMPLATE.md
@@ -1,55 +1,42 @@
-## PR Type
-
-<!-- Select one and remove others. If an appropriate one is not listed, please write by yourself. -->
-
-- New Feature
-- Improvement
-- Bug Fix
-
-## Related Links
-
-<!-- Please write related links to GitHub/Jira/Slack/etc. -->
-
 ## Description
 
-<!-- Describe what this PR changes. -->
+<!-- Write a brief description of this PR. -->
+
+## Related links
 
-## Review Procedure
+<!-- Write the links related to this PR. -->
 
-<!-- Explain how to review this PR. -->
+## Tests performed
 
-## Remarks
+<!-- Describe how you have tested this PR. -->
 
-<!-- Write remarks as you like if you need them. -->
+## Notes for reviewers
 
-## Pre-Review Checklist for the PR Author
+<!-- Write additional information if necessary. It should be written if there are related PRs that should be merged at the same time. -->
 
-**PR Author should check the checkboxes below when creating the PR.**
+## Pre-review checklist for the PR author
 
-- [ ] Code follows [coding guidelines][coding-guidelines]
-- [ ] Assign PR to reviewer
+The PR author **must** check the checkboxes below when creating the PR.
 
-## Checklist for the PR Reviewer
+- [ ] I've confirmed the [contribution guidelines].
+- [ ] The PR follows the [pull request guidelines].
 
-**Reviewers should check the checkboxes below before approval.**
+## In-review checklist for the PR reviewers
 
-- [ ] Commits are properly organized and messages are according to the guideline
-- [ ] Code follows [coding guidelines][coding-guidelines]
-- [ ] (Optional) Unit tests have been written for new behavior
-- [ ] PR title describes the changes
+The PR reviewers **must** check the checkboxes below before approval.
 
-## Post-Review Checklist for the PR Author
+- [ ] The PR follows the [pull request guidelines].
+- [ ] The PR has been properly tested.
+- [ ] The PR has been reviewed by the code owners.
 
-**PR Author should check the checkboxes below before merging.**
+## Post-review checklist for the PR author
 
-- [ ] All open points are addressed and tracked via issues or tickets
-- [ ] Write [release notes][release-notes]
+The PR author **must** check the checkboxes below before merging.
 
-## CI Checks
+- [ ] There are no open discussions or they are tracked via tickets.
+- [ ] The PR is ready for merge.
 
-- **Build and test for PR**: Required to pass before the merge.
-- **Check spelling**: NOT required to pass before the merge. It is up to the reviewer(s). See [here][spell-check-dict] if you want to add some words to the spell check dictionary.
+After all checkboxes are checked, anyone who has write access can merge the PR.
 
-[coding-guidelines]: https://tier4.atlassian.net/wiki/spaces/AIP/pages/1194394777/T4
-[release-notes]: https://tier4.atlassian.net/l/c/X1p69s6B
-[spell-check-dict]: https://github.com/tier4/autoware-spell-check-dict#how-to-contribute
+[contribution guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/
+[pull request guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/
\ No newline at end of file

From dbe78844f91a09fa92bd9f76d07e892da49099ce Mon Sep 17 00:00:00 2001
From: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
Date: Thu, 7 Jul 2022 13:31:38 +0900
Subject: [PATCH 03/20] fix: x1 base-lidar launcher

---
 .../aip_x1/ground_plane_sensor_kit.launch.xml | 22 +------------------
 1 file changed, 1 insertion(+), 21 deletions(-)

diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane_sensor_kit.launch.xml
index bd65fe9a..47785137 100644
--- a/sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane_sensor_kit.launch.xml
+++ b/sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane_sensor_kit.launch.xml
@@ -22,9 +22,7 @@
   <node pkg="extrinsic_calibration_manager" exec="extrinsic_calibration_manager" name="extrinsic_calibration_manager" output="screen">
     <param name="parent_frame" value="$(var parent_frame)" />
     <param name="child_frames" value="
-    [velodyne_top_base_link,
-    livox_front_center_base_link,
-    livox_front_right_base_link]" />
+    [velodyne_top_base_link]" />
   </node>
 
   <!-- velodyne_top_base_link: extrinsic_ground_plane_calibrator -->
@@ -40,22 +38,4 @@
 
   <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_profile)" if="$(var rviz)"/>
 
-  <!-- livox_front_center_base_link: extrinsic_dummy_calibrator -->
-  <group>
-    <include file="$(find-pkg-share extrinsic_dummy_calibrator)/launch/calibrator.launch.xml">
-      <arg name="ns" value="$(var parent_frame)/livox_front_center_base_link" />
-      <arg name="parent_frame" value="$(var parent_frame)" />
-      <arg name="child_frame" value="livox_front_center_base_link" />
-    </include>
-  </group>
-
-  <!-- livox_front_right_base_link: extrinsic_dummy_calibrator -->
-  <group>
-    <include file="$(find-pkg-share extrinsic_dummy_calibrator)/launch/calibrator.launch.xml">
-      <arg name="ns" value="$(var parent_frame)/livox_front_right_base_link" />
-      <arg name="parent_frame" value="$(var parent_frame)" />
-      <arg name="child_frame" value="livox_front_right_base_link" />
-    </include>
-  </group>
-
 </launch>

From a9c0fb888a448267f4009d0c13980ccbb417fcba Mon Sep 17 00:00:00 2001
From: kminoda <koji.m.minoda@gmail.com>
Date: Thu, 7 Jul 2022 15:31:06 +0900
Subject: [PATCH 04/20] fix(deviation_evaluator): debugged map_launch in launch
 file

Signed-off-by: kminoda <koji.m.minoda@gmail.com>
---
 .../launch/deviation_evaluator.launch.xml              | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
index 8497ca4b..95448ec7 100644
--- a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
+++ b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
@@ -13,8 +13,7 @@
   <arg name="out_ekf_odom" default="/localization/deviation_evaluator/ekf_localizer/kinematic_state"/>
   <arg name="out_twist_with_covariance" default="/localization/deviation_evaluator/twist_estimator/twist_with_covariance"/>
   <arg name="out_pose_with_covariance" default="/localization/deviation_evaluator/pose_estimator/pose_with_covariance"/>
-  <arg name="lanelet2_map_file" default="lanelet2_map.osm" />
-  <arg name="pointcloud_map_file" default="pointcloud_map.pcd" />
+  <arg name="lanelet2_map_file" default="$(var map_path)/lanelet2_map.osm" />
 
   <node pkg="deviation_evaluator" exec="deviation_evaluator" name="deviation_evaluator" output="screen">
     <remap from="in_twist_with_covariance" to="$(var in_twist_with_covariance)"/>
@@ -52,9 +51,10 @@
 
   <!-- Map -->
   <group>
-    <include file="$(find-pkg-share map_launch)/launch/map.launch.py" if="$(var rviz)">
-      <arg name="lanelet2_map_path" value="$(var map_path)/$(var lanelet2_map_file)" />
-      <arg name="pointcloud_map_path" value="$(var map_path)/$(var pointcloud_map_file)"/>
+    <include file="$(find-pkg-share map_loader)/launch/lanelet2_map_loader.launch.xml" if="$(var rviz)">
+      <arg name="lanelet2_map_path" value="$(var lanelet2_map_file)"/>
+      <arg name="lanelet2_map_topic" value="/map/vector_map"/>
+      <arg name="lanelet2_map_marker_topic" value="/map/vector_map_marker"/>
     </include>
   </group>
 

From 925fbc4deb4340736ed14b34919ff9c6bc57b85c Mon Sep 17 00:00:00 2001
From: Akihito OHSATO <aohsato@gmail.com>
Date: Thu, 7 Jul 2022 16:24:19 +0900
Subject: [PATCH 05/20] fix README to refer to own repos file

Signed-off-by: Akihito OHSATO <aohsato@gmail.com>
---
 README.md | 17 +++++++----------
 1 file changed, 7 insertions(+), 10 deletions(-)

diff --git a/README.md b/README.md
index bb3f0801..624dde76 100644
--- a/README.md
+++ b/README.md
@@ -4,21 +4,18 @@ Calibration tools used for autonomous driving
 
 ## Requirement
 
-- Ubuntu20.04
-- Ros Galactic
-
+- Ubuntu 20.04
+- ROS 2 Galactic
 
 ## Installation procedures
 
-After cloning pilot-auto(private repository) or [autoware](https://github.com/tier4/autoware), execute the following commands:
+After installing [autoware](https://github.com/tier4/autoware) (please see [source-installation](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/) page), execute the following commands:
 
 ```bash
-mkdir src
-vcs import src < autoware.repos
-cd src/autoware
-git clone git@github.com:tier4/CalibrationTools.git
-cd ../../
-vcs import src < src/autoware/calibration_tools/calibration_tools.repos
+cd autoware
+wget https://raw.githubusercontent.com/tier4/CalibrationTools/tier4/universe/calibration_tools.repos
+vcs import src < calibration_tools.repos
+rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
 colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```
 

From a00c94d962afbf088e21db0b3e1362a256e5ea61 Mon Sep 17 00:00:00 2001
From: Akihito OHSATO <aohsato@gmail.com>
Date: Thu, 7 Jul 2022 16:30:57 +0900
Subject: [PATCH 06/20] fix repos file to fit autoware build process

Signed-off-by: Akihito OHSATO <aohsato@gmail.com>
---
 calibration_tools.repos | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/calibration_tools.repos b/calibration_tools.repos
index 738c26dd..191ebc0d 100644
--- a/calibration_tools.repos
+++ b/calibration_tools.repos
@@ -1,4 +1,8 @@
 repositories:
+  autoware/calibration_tools:
+    type: git
+    url: git@github.com:tier4/calibration_tools.git
+    version: tier4/universe
   vendor/lidartag:
     type: git
     url: git@github.com:tier4/LiDARTag.git
@@ -19,3 +23,7 @@ repositories:
     type: git
     url: git@github.com:Box-Robotics/ros2_numpy.git
     version: rolling
+  vendor/image_pipeline:
+    type: git
+    url: https://github.com/tier4/image_pipeline.git
+    version: 47964112293eb19f9f57254b2e6b68706954cc63

From a3111bde3cf0ed984789703a9dbff41b7fd0db2a Mon Sep 17 00:00:00 2001
From: Akihito OHSATO <aohsato@gmail.com>
Date: Thu, 7 Jul 2022 18:20:33 +0900
Subject: [PATCH 07/20] fix calibration_tools url

Signed-off-by: Akihito OHSATO <aohsato@gmail.com>
---
 calibration_tools.repos | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/calibration_tools.repos b/calibration_tools.repos
index 191ebc0d..5846824b 100644
--- a/calibration_tools.repos
+++ b/calibration_tools.repos
@@ -1,7 +1,7 @@
 repositories:
   autoware/calibration_tools:
     type: git
-    url: git@github.com:tier4/calibration_tools.git
+    url: git@github.com:tier4/CalibrationTools.git
     version: tier4/universe
   vendor/lidartag:
     type: git

From 4d29700e2cf1b7bb8a1f7ebb536e6d2293563f96 Mon Sep 17 00:00:00 2001
From: Akihito OHSATO <aohsato@gmail.com>
Date: Thu, 7 Jul 2022 18:26:12 +0900
Subject: [PATCH 08/20] remove installation from sensor/readme

Signed-off-by: Akihito OHSATO <aohsato@gmail.com>
---
 sensor/README.md | 11 -----------
 1 file changed, 11 deletions(-)

diff --git a/sensor/README.md b/sensor/README.md
index 28a9cf26..ffe32523 100644
--- a/sensor/README.md
+++ b/sensor/README.md
@@ -1,16 +1,5 @@
 # Sensor Calibration Tools
 
-## Calibration tools installation (alongside autoware)
-
-After cloning autoware, execute the following commands:
-
-```sh
-mkdir src
-vcs import src < autoware.proj.repos
-vcs import src < src/autoware/calibration_tools/calibration_tools.repos
-colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
-```
-
 ## Extrinsic Calibration
 
 - [How to Extrinsic Manual Calibration](docs/how_to_extrinsic_manual.md)

From 25b3e7da56a1f6cc2bf262ca6af04e0a53e5dff5 Mon Sep 17 00:00:00 2001
From: Akihito OHSATO <aohsato@gmail.com>
Date: Thu, 7 Jul 2022 18:28:47 +0900
Subject: [PATCH 09/20] modify readme urls to relative path

Signed-off-by: Akihito OHSATO <aohsato@gmail.com>
---
 README.md | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/README.md b/README.md
index 624dde76..97f184ef 100644
--- a/README.md
+++ b/README.md
@@ -25,28 +25,28 @@ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
 
 We provide calibration tool for sensor pairs like LiDAR - LiDAR, LiDAR - Camera, etc.
 
-[README](https://github.com/tier4/calibration_tools.iv/blob/tier4/universe/sensor/README.md)
+[README](sensor/README.md)
 
 ### localization - deviation estimation tools
 
 Estimate parameters of sensors used for dead reckoning (IMU and odometry) for a better localization performance
 
-[README](https://github.com/tier4/calibration_tools.iv/blob/tier4/universe/localization/deviation_estimation_tools/ReadMe.md)
+[README](localization/deviation_estimation_tools/ReadMe.md)
 
 ### control - vehicle cmd analyzer
 
 Visualization and analysis tools for the control outputs from Autoware
 
-[README](https://github.com/tier4/calibration_tools.iv/blob/tier4/universe/control/vehicle_cmd_analyzer/README.md)
+[README](control/vehicle_cmd_analyzer/README.md)
 
 ### vehicle - time delay estimator
 
 Calibration tool to fix the delay of the commands to the vehicle
 
-[README](https://github.com/tier4/calibration_tools.iv/blob/tier4/universe/vehicle/time_delay_estimator/README.md)
+[README](vehicle/time_delay_estimator/README.md)
 
 ### system - tunable static tf broadcaster
 
 GUI to modify the parameters of generic TFs.
 
-[README](https://github.com/tier4/calibration_tools.iv/blob/tier4/universe/system/tunable_static_tf_broadcaster/README.md)
+[README](system/tunable_static_tf_broadcaster/README.md)

From 84e41fe00127d78e78b90c34d84482a12505efc3 Mon Sep 17 00:00:00 2001
From: kminoda <koji.m.minoda@gmail.com>
Date: Fri, 8 Jul 2022 11:43:02 +0900
Subject: [PATCH 10/20] fix(deviation_estimation_tools): fix a bug in loading
 parameter file

Signed-off-by: kminoda <koji.m.minoda@gmail.com>
---
 .../deviation_estimation_tools/ReadMe.md      | 24 +++++++++++--------
 ...or.yaml => deviation_evaluator.param.yaml} |  2 +-
 .../launch/deviation_evaluator.launch.xml     | 12 +++++-----
 3 files changed, 21 insertions(+), 17 deletions(-)
 rename localization/deviation_estimation_tools/deviation_evaluator/config/{deviation_evaluator.yaml => deviation_evaluator.param.yaml} (92%)

diff --git a/localization/deviation_estimation_tools/ReadMe.md b/localization/deviation_estimation_tools/ReadMe.md
index bb6fc9f8..2d88d3eb 100644
--- a/localization/deviation_estimation_tools/ReadMe.md
+++ b/localization/deviation_estimation_tools/ReadMe.md
@@ -30,6 +30,7 @@ If your are using rosbag, it should contain the following topics:
 - /localization/pose_estimator/pose_with_covariance
 - /clock
 - /tf_static (that contains transform from `base_link` to `imu_link`)
+- (/localization/twist_estimator/twist_with_covariance: required in the next step)
 - (/localization/pose_twist_fusion_filter/kinematic_state: required in the next step)
 
 Play the rosbag in a different terminal:
@@ -43,19 +44,22 @@ ros2 bag play YOUR_BAG # You can also play in a faster rate, e.g. -r 5
 
 ```sh
 Files:             localized_not_calibrated_0.db3
-Bag size:          11.1 MiB
+Bag size:          9.6 MiB
 Storage id:        sqlite3
-Duration:          151.303s
-Start:             Apr 20 2022 20:49:10.440 (1650455350.440)
-End:               Apr 20 2022 20:51:41.744 (1650455501.744)
-Messages:          53400
-Topic information: Topic: /tf | Type: tf2_msgs/msg/TFMessage | Count: 9806 | Serialization Format: cdr
-                   Topic: /clock | Type: rosgraph_msgs/msg/Clock | Count: 30260 | Serialization Format: cdr
-                   Topic: /localization/twist_estimator/twist_with_covariance | Type: geometry_msgs/msg/TwistWithCovarianceStamped | Count: 4281 | Serialization Format: cdr
-                   Topic: /localization/pose_estimator/pose_with_covariance | Type: geometry_msgs/msg/PoseWithCovarianceStamped | Count: 1484 | Serialization Format: cdr
-                   Topic: /localization/pose_twist_fusion_filter/kinematic_state | Type: nav_msgs/msg/Odometry | Count: 7565 | Serialization Format: cdr
+Duration:          76.539s
+Start:             Jul  8 2022 11:21:41.220 (1657246901.220)
+End:               Jul  8 2022 11:22:57.759 (1657246977.759)
+Messages:          32855
+Topic information: Topic: /localization/twist_estimator/twist_with_covariance_raw | Type: geometry_msgs/msg/TwistWithCovarianceStamped | Count: 2139 | Serialization Format: cdr
+                   Topic: /localization/kinematic_state | Type: nav_msgs/msg/Odometry | Count: 3792 | Serialization Format: cdr
+                   Topic: /localization/twist_estimator/twist_with_covariance | Type: geometry_msgs/msg/TwistWithCovarianceStamped | Count: 2139 | Serialization Format: cdr
+                   Topic: /clock | Type: rosgraph_msgs/msg/Clock | Count: 15308 | Serialization Format: cdr
+                   Topic: /tf | Type: tf2_msgs/msg/TFMessage | Count: 4950 | Serialization Format: cdr
+                   Topic: /localization/pose_twist_fusion_filter/kinematic_state | Type: nav_msgs/msg/Odometry | Count: 3792 | Serialization Format: cdr
+                   Topic: /localization/pose_estimator/pose_with_covariance | Type: geometry_msgs/msg/PoseWithCovarianceStamped | Count: 731 | Serialization Format: cdr
                    Topic: /tf_static | Type: tf2_msgs/msg/TFMessage | Count: 4 | Serialization Format: cdr
 
+
 ```
 
 </p>
diff --git a/localization/deviation_estimation_tools/deviation_evaluator/config/deviation_evaluator.yaml b/localization/deviation_estimation_tools/deviation_evaluator/config/deviation_evaluator.param.yaml
similarity index 92%
rename from localization/deviation_estimation_tools/deviation_evaluator/config/deviation_evaluator.yaml
rename to localization/deviation_estimation_tools/deviation_evaluator/config/deviation_evaluator.param.yaml
index 994737d6..4e3aa14b 100644
--- a/localization/deviation_estimation_tools/deviation_evaluator/config/deviation_evaluator.yaml
+++ b/localization/deviation_estimation_tools/deviation_evaluator/config/deviation_evaluator.param.yaml
@@ -1,4 +1,4 @@
-parameter_estimator:
+/**:
   ros__parameters:
     # Parameters that you want to evaluate
     stddev_vx: 0.3 # [m/s]
diff --git a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
index 8497ca4b..916dd861 100644
--- a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
+++ b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
@@ -5,7 +5,7 @@
   <arg name="rviz" default="false" description="launch rviz"/>
   <arg name="map_path" default="" description="point cloud and lanelet2 map directory path"/>
   <arg name="save_dir" default="/home/$(env USER)/deviation_evaluator_sample"/>
-  <arg name="param_path" default="$(find-pkg-share deviation_evaluator)/config/deviation_evaluator.yaml"/>
+  <arg name="param_path" default="$(find-pkg-share deviation_evaluator)/config/deviation_evaluator.param.yaml"/>
 
   <arg name="in_twist_with_covariance" default="/localization/twist_estimator/twist_with_covariance"/>
   <arg name="in_ndt_pose_with_covariance" default="/localization/pose_estimator/pose_with_covariance"/>
@@ -13,8 +13,7 @@
   <arg name="out_ekf_odom" default="/localization/deviation_evaluator/ekf_localizer/kinematic_state"/>
   <arg name="out_twist_with_covariance" default="/localization/deviation_evaluator/twist_estimator/twist_with_covariance"/>
   <arg name="out_pose_with_covariance" default="/localization/deviation_evaluator/pose_estimator/pose_with_covariance"/>
-  <arg name="lanelet2_map_file" default="lanelet2_map.osm" />
-  <arg name="pointcloud_map_file" default="pointcloud_map.pcd" />
+  <arg name="lanelet2_map_file" default="$(var map_path)/lanelet2_map.osm" />
 
   <node pkg="deviation_evaluator" exec="deviation_evaluator" name="deviation_evaluator" output="screen">
     <remap from="in_twist_with_covariance" to="$(var in_twist_with_covariance)"/>
@@ -52,9 +51,10 @@
 
   <!-- Map -->
   <group>
-    <include file="$(find-pkg-share map_launch)/launch/map.launch.py" if="$(var rviz)">
-      <arg name="lanelet2_map_path" value="$(var map_path)/$(var lanelet2_map_file)" />
-      <arg name="pointcloud_map_path" value="$(var map_path)/$(var pointcloud_map_file)"/>
+    <include file="$(find-pkg-share map_loader)/launch/lanelet2_map_loader.launch.xml" if="$(var rviz)">
+      <arg name="lanelet2_map_path" value="$(var lanelet2_map_file)"/>
+      <arg name="lanelet2_map_topic" value="/map/vector_map"/>
+      <arg name="lanelet2_map_marker_topic" value="/map/vector_map_marker"/>
     </include>
   </group>
 

From 12e67da3432356873bd1c6f5e670de97508f9108 Mon Sep 17 00:00:00 2001
From: kminoda <koji.m.minoda@gmail.com>
Date: Fri, 8 Jul 2022 11:45:54 +0900
Subject: [PATCH 11/20] reverted a change around map loader

Signed-off-by: kminoda <koji.m.minoda@gmail.com>
---
 .../launch/deviation_evaluator.launch.xml            | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
index 916dd861..8497ca4b 100644
--- a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
+++ b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
@@ -5,7 +5,7 @@
   <arg name="rviz" default="false" description="launch rviz"/>
   <arg name="map_path" default="" description="point cloud and lanelet2 map directory path"/>
   <arg name="save_dir" default="/home/$(env USER)/deviation_evaluator_sample"/>
-  <arg name="param_path" default="$(find-pkg-share deviation_evaluator)/config/deviation_evaluator.param.yaml"/>
+  <arg name="param_path" default="$(find-pkg-share deviation_evaluator)/config/deviation_evaluator.yaml"/>
 
   <arg name="in_twist_with_covariance" default="/localization/twist_estimator/twist_with_covariance"/>
   <arg name="in_ndt_pose_with_covariance" default="/localization/pose_estimator/pose_with_covariance"/>
@@ -13,7 +13,8 @@
   <arg name="out_ekf_odom" default="/localization/deviation_evaluator/ekf_localizer/kinematic_state"/>
   <arg name="out_twist_with_covariance" default="/localization/deviation_evaluator/twist_estimator/twist_with_covariance"/>
   <arg name="out_pose_with_covariance" default="/localization/deviation_evaluator/pose_estimator/pose_with_covariance"/>
-  <arg name="lanelet2_map_file" default="$(var map_path)/lanelet2_map.osm" />
+  <arg name="lanelet2_map_file" default="lanelet2_map.osm" />
+  <arg name="pointcloud_map_file" default="pointcloud_map.pcd" />
 
   <node pkg="deviation_evaluator" exec="deviation_evaluator" name="deviation_evaluator" output="screen">
     <remap from="in_twist_with_covariance" to="$(var in_twist_with_covariance)"/>
@@ -51,10 +52,9 @@
 
   <!-- Map -->
   <group>
-    <include file="$(find-pkg-share map_loader)/launch/lanelet2_map_loader.launch.xml" if="$(var rviz)">
-      <arg name="lanelet2_map_path" value="$(var lanelet2_map_file)"/>
-      <arg name="lanelet2_map_topic" value="/map/vector_map"/>
-      <arg name="lanelet2_map_marker_topic" value="/map/vector_map_marker"/>
+    <include file="$(find-pkg-share map_launch)/launch/map.launch.py" if="$(var rviz)">
+      <arg name="lanelet2_map_path" value="$(var map_path)/$(var lanelet2_map_file)" />
+      <arg name="pointcloud_map_path" value="$(var map_path)/$(var pointcloud_map_file)"/>
     </include>
   </group>
 

From 196e5094642ee1d0db95cd274bbdd2e3c4be29e9 Mon Sep 17 00:00:00 2001
From: kminoda <koji.m.minoda@gmail.com>
Date: Fri, 8 Jul 2022 11:47:26 +0900
Subject: [PATCH 12/20] revert readme modification

Signed-off-by: kminoda <koji.m.minoda@gmail.com>
---
 .../deviation_estimation_tools/ReadMe.md      | 24 ++++++++-----------
 1 file changed, 10 insertions(+), 14 deletions(-)

diff --git a/localization/deviation_estimation_tools/ReadMe.md b/localization/deviation_estimation_tools/ReadMe.md
index 2d88d3eb..bb6fc9f8 100644
--- a/localization/deviation_estimation_tools/ReadMe.md
+++ b/localization/deviation_estimation_tools/ReadMe.md
@@ -30,7 +30,6 @@ If your are using rosbag, it should contain the following topics:
 - /localization/pose_estimator/pose_with_covariance
 - /clock
 - /tf_static (that contains transform from `base_link` to `imu_link`)
-- (/localization/twist_estimator/twist_with_covariance: required in the next step)
 - (/localization/pose_twist_fusion_filter/kinematic_state: required in the next step)
 
 Play the rosbag in a different terminal:
@@ -44,22 +43,19 @@ ros2 bag play YOUR_BAG # You can also play in a faster rate, e.g. -r 5
 
 ```sh
 Files:             localized_not_calibrated_0.db3
-Bag size:          9.6 MiB
+Bag size:          11.1 MiB
 Storage id:        sqlite3
-Duration:          76.539s
-Start:             Jul  8 2022 11:21:41.220 (1657246901.220)
-End:               Jul  8 2022 11:22:57.759 (1657246977.759)
-Messages:          32855
-Topic information: Topic: /localization/twist_estimator/twist_with_covariance_raw | Type: geometry_msgs/msg/TwistWithCovarianceStamped | Count: 2139 | Serialization Format: cdr
-                   Topic: /localization/kinematic_state | Type: nav_msgs/msg/Odometry | Count: 3792 | Serialization Format: cdr
-                   Topic: /localization/twist_estimator/twist_with_covariance | Type: geometry_msgs/msg/TwistWithCovarianceStamped | Count: 2139 | Serialization Format: cdr
-                   Topic: /clock | Type: rosgraph_msgs/msg/Clock | Count: 15308 | Serialization Format: cdr
-                   Topic: /tf | Type: tf2_msgs/msg/TFMessage | Count: 4950 | Serialization Format: cdr
-                   Topic: /localization/pose_twist_fusion_filter/kinematic_state | Type: nav_msgs/msg/Odometry | Count: 3792 | Serialization Format: cdr
-                   Topic: /localization/pose_estimator/pose_with_covariance | Type: geometry_msgs/msg/PoseWithCovarianceStamped | Count: 731 | Serialization Format: cdr
+Duration:          151.303s
+Start:             Apr 20 2022 20:49:10.440 (1650455350.440)
+End:               Apr 20 2022 20:51:41.744 (1650455501.744)
+Messages:          53400
+Topic information: Topic: /tf | Type: tf2_msgs/msg/TFMessage | Count: 9806 | Serialization Format: cdr
+                   Topic: /clock | Type: rosgraph_msgs/msg/Clock | Count: 30260 | Serialization Format: cdr
+                   Topic: /localization/twist_estimator/twist_with_covariance | Type: geometry_msgs/msg/TwistWithCovarianceStamped | Count: 4281 | Serialization Format: cdr
+                   Topic: /localization/pose_estimator/pose_with_covariance | Type: geometry_msgs/msg/PoseWithCovarianceStamped | Count: 1484 | Serialization Format: cdr
+                   Topic: /localization/pose_twist_fusion_filter/kinematic_state | Type: nav_msgs/msg/Odometry | Count: 7565 | Serialization Format: cdr
                    Topic: /tf_static | Type: tf2_msgs/msg/TFMessage | Count: 4 | Serialization Format: cdr
 
-
 ```
 
 </p>

From 9e95235f1e2193b67d0c5bd2f0b2cb13e7657941 Mon Sep 17 00:00:00 2001
From: kminoda <koji.m.minoda@gmail.com>
Date: Fri, 8 Jul 2022 11:48:07 +0900
Subject: [PATCH 13/20] changed param file name in launch file

Signed-off-by: kminoda <koji.m.minoda@gmail.com>
---
 .../deviation_evaluator/launch/deviation_evaluator.launch.xml   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
index 8497ca4b..666907cb 100644
--- a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
+++ b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml
@@ -5,7 +5,7 @@
   <arg name="rviz" default="false" description="launch rviz"/>
   <arg name="map_path" default="" description="point cloud and lanelet2 map directory path"/>
   <arg name="save_dir" default="/home/$(env USER)/deviation_evaluator_sample"/>
-  <arg name="param_path" default="$(find-pkg-share deviation_evaluator)/config/deviation_evaluator.yaml"/>
+  <arg name="param_path" default="$(find-pkg-share deviation_evaluator)/config/deviation_evaluator.param.yaml"/>
 
   <arg name="in_twist_with_covariance" default="/localization/twist_estimator/twist_with_covariance"/>
   <arg name="in_ndt_pose_with_covariance" default="/localization/pose_estimator/pose_with_covariance"/>

From a93d2288818f7d086e57094388084ccf09264e8f Mon Sep 17 00:00:00 2001
From: kminoda <koji.m.minoda@gmail.com>
Date: Fri, 8 Jul 2022 11:50:14 +0900
Subject: [PATCH 14/20] fix(deviation_estimation_tools): added a neccesary
 topic name in readme

Signed-off-by: kminoda <koji.m.minoda@gmail.com>
---
 .../deviation_estimation_tools/ReadMe.md      | 23 +++++++++++--------
 1 file changed, 13 insertions(+), 10 deletions(-)

diff --git a/localization/deviation_estimation_tools/ReadMe.md b/localization/deviation_estimation_tools/ReadMe.md
index bb6fc9f8..73580e19 100644
--- a/localization/deviation_estimation_tools/ReadMe.md
+++ b/localization/deviation_estimation_tools/ReadMe.md
@@ -30,6 +30,7 @@ If your are using rosbag, it should contain the following topics:
 - /localization/pose_estimator/pose_with_covariance
 - /clock
 - /tf_static (that contains transform from `base_link` to `imu_link`)
+- (/localization/twist_estimator/twist_with_covariance: required in the next step)
 - (/localization/pose_twist_fusion_filter/kinematic_state: required in the next step)
 
 Play the rosbag in a different terminal:
@@ -43,17 +44,19 @@ ros2 bag play YOUR_BAG # You can also play in a faster rate, e.g. -r 5
 
 ```sh
 Files:             localized_not_calibrated_0.db3
-Bag size:          11.1 MiB
+Bag size:          9.6 MiB
 Storage id:        sqlite3
-Duration:          151.303s
-Start:             Apr 20 2022 20:49:10.440 (1650455350.440)
-End:               Apr 20 2022 20:51:41.744 (1650455501.744)
-Messages:          53400
-Topic information: Topic: /tf | Type: tf2_msgs/msg/TFMessage | Count: 9806 | Serialization Format: cdr
-                   Topic: /clock | Type: rosgraph_msgs/msg/Clock | Count: 30260 | Serialization Format: cdr
-                   Topic: /localization/twist_estimator/twist_with_covariance | Type: geometry_msgs/msg/TwistWithCovarianceStamped | Count: 4281 | Serialization Format: cdr
-                   Topic: /localization/pose_estimator/pose_with_covariance | Type: geometry_msgs/msg/PoseWithCovarianceStamped | Count: 1484 | Serialization Format: cdr
-                   Topic: /localization/pose_twist_fusion_filter/kinematic_state | Type: nav_msgs/msg/Odometry | Count: 7565 | Serialization Format: cdr
+Duration:          76.539s
+Start:             Jul  8 2022 11:21:41.220 (1657246901.220)
+End:               Jul  8 2022 11:22:57.759 (1657246977.759)
+Messages:          32855
+Topic information: Topic: /localization/twist_estimator/twist_with_covariance_raw | Type: geometry_msgs/msg/TwistWithCovarianceStamped | Count: 2139 | Serialization Format: cdr
+                   Topic: /localization/kinematic_state | Type: nav_msgs/msg/Odometry | Count: 3792 | Serialization Format: cdr
+                   Topic: /localization/twist_estimator/twist_with_covariance | Type: geometry_msgs/msg/TwistWithCovarianceStamped | Count: 2139 | Serialization Format: cdr
+                   Topic: /clock | Type: rosgraph_msgs/msg/Clock | Count: 15308 | Serialization Format: cdr
+                   Topic: /tf | Type: tf2_msgs/msg/TFMessage | Count: 4950 | Serialization Format: cdr
+                   Topic: /localization/pose_twist_fusion_filter/kinematic_state | Type: nav_msgs/msg/Odometry | Count: 3792 | Serialization Format: cdr
+                   Topic: /localization/pose_estimator/pose_with_covariance | Type: geometry_msgs/msg/PoseWithCovarianceStamped | Count: 731 | Serialization Format: cdr
                    Topic: /tf_static | Type: tf2_msgs/msg/TFMessage | Count: 4 | Serialization Format: cdr
 
 ```

From 040dda93c47814c883d8ed6d6d3cfd1fef8d670a Mon Sep 17 00:00:00 2001
From: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>
Date: Fri, 15 Jul 2022 11:14:27 +0900
Subject: [PATCH 15/20] feat: sensor documentation (#14)

* Added a documentation section to the sensor section reflecting our calibration design and a brief explanation of each calibration method

* Attempting to use underscores correctly in math mode. It works correctly on local but on github appears as an error

* Trying using the underscores outside math mode

* Another attempt

* mend

* mend

* mend

* mend

* mend

* mend

* mend

* Decided agains using "traditional underscores"
---
 sensor/README.md                              | 89 +++++++++++++++++--
 sensor/docs/design/sensor_calibration.svg     |  4 +
 ..._auto.md => how_to_extrinsic_map_based.md} |  2 +-
 3 files changed, 87 insertions(+), 8 deletions(-)
 create mode 100644 sensor/docs/design/sensor_calibration.svg
 rename sensor/docs/{how_to_extrinsic_auto.md => how_to_extrinsic_map_based.md} (98%)

diff --git a/sensor/README.md b/sensor/README.md
index ffe32523..2c9810d7 100644
--- a/sensor/README.md
+++ b/sensor/README.md
@@ -1,19 +1,94 @@
 # Sensor Calibration Tools
 
+Sensor calibration can be split into two categories: intrinsic sensor calibration and extrinsic sensor calibration. In our calibration tools, we implement different methods for both categories.
+
 ## Extrinsic Calibration
 
-- [How to Extrinsic Manual Calibration](docs/how_to_extrinsic_manual.md)
+Extrinsic calibration refers to the relative poses among sensors that are provided to ROS2 nodes using the TF interface. Our calibration tools assume a TF diagram like the one presented in Figure 1.
+
+<figure align="center">
+   <img src="docs/design/sensor_calibration.svg" width="600">
+   <figcaption align="center">Figure 1. Extrinsic calibration diagram</figcaption>
+</figure>
+
+In our design, `base_link` corresponds to the projection on the ground of the vehicle's rear axis and each vehicle may possess one or more sensor kits, which is a physical location on the vehicle where sensors are mounted. For example, a normal car would possess one sensor kit mounted on its top, whereas larger vehicles (e.g., a bus) would have several sensor kits distributed along the vehicle.
+
+Although in the diagram present in Figure 1 the TFs from the base to the sensor kits, and from each kit to its sensors are provided, these are not the calibration targets. Instead, we calibrate from the `base_link` to a particular lidar, then from said lidar to the rest of the lidars, and finally from lidars to cameras. In order to comply with the diagram from Figure 1, the final output of the calibration process becomes:
+
+$T(\text{sensor⎽kit⎽base⎽link}, \text{lidar0⎽base⎽link}) = T(\text{base⎽link}, \text{sensor⎽kit⎽base⎽link})^{-1} \times T(\text{base⎽link}, \text{lidar0⎽base⎽link})$
+
+$T(\text{sensor⎽kit⎽base⎽link}, \text{lidar1⎽base⎽link}) = T(\text{sensor⎽kit⎽base⎽link}, \text{lidar0⎽base⎽link}) \times T(\text{lidar0⎽base⎽link}, \text{lidar1⎽base⎽link})$
+
+$T(\text{sensor⎽kit⎽base⎽link}, \text{camera0/camera⎽link}) = T(\text{sensor⎽kit⎽base⎽link}, \text{lidar0⎽base⎽link}) \times T(\text{lidar0⎽base⎽link}, \text{camera0/camera⎽link})$
+
+where the $T(\text{base⎽link}, \text{sensor⎽kit⎽base⎽link})$ is usually provided by a CAD model or can be simply approximated since it is a convenient frame and does not affect other computations.
+
+Looking at the diagram from Figure 1, we could also directly calibrate all the sensors with respect to the `base_link`. However, we believe that sensor-sensor calibration provides more accurate and consistent results so we only use one `base_link` to sensor calibration and from then all other calibrations are performed via pairs of sensors.
+
+### Generic calibration
+
+- [Extrinsic manual calibration](docs/how_to_extrinsic_manual.md)
+
+Intended as a proof-of-concept of our calibration API and as a baseline to which to compare automatic calibration tools, this method allows us to directly modify the values of the TF tree with a rviz view to evaluate the tfs and calibration.
+
+<figure align="center">
+   <img src="docs/images/rviz_lidar.png" width="400">
+   <figcaption align="center">Figure 2. Manual calibration</figcaption>
+</figure>
+
+### Base-lidar calibration
 
-- [How to Extrinsic Automatic Calibration](docs/how_to_extrinsic_auto.md)
+- [Ground-plane base-lidar calibration](docs/how_to_extrinsic_ground_plane.md)
 
-- [How to Extrinsic Interactive Calibration](docs/how_to_extrinsic_interactive.md)
+This calibration method assumes the floor around the vehicle forms a plane and adjusts the calibration tf so that the points corresponding to the ground of the point cloud lie on the XY plane of the `base_link`. As such, only the `z`, `roll`, and `pitch` values of the tf are calibrated, with the remaining `x`, `y`, and `yaw` values needing to be calibrated via other methods such as manual calibration.
 
-- [How to Extrinsic Tag-based Calibration](docs/how_to_extrinsic_tag_based.md)
+<figure align="center">
+   <img src="docs/images/base-lidar/initial_calibration.png" width="250">
+   <img src="docs/images/base-lidar/final_calibration.png" width="250">
+   <figcaption align="center">Figure 3. Ground-plane base-lidar calibration (before and after calibration)</figcaption>
+</figure>
 
-- [How to Extrinsic Ground-plane Calibration](docs/how_to_extrinsic_ground_plane.md)
+### Lidar-lidar calibration
+
+- [Map-based lidar-lidar calibration](docs/how_to_extrinsic_map_based.md)
+
+To calibrate pairs of lidars, this method uses standard point cloud registration. However, due to the sparsity of traditional lidars, direct registration proves difficult. To address this issue, this method uses an additional point cloud map and a localization system to solvent this limitation during registration.
+
+<figure align="center">
+   <img src="docs/images/map_based_calibration_results.png" width="400">
+   <figcaption align="center">Figure 4. Map-based calibration</figcaption>
+</figure>
+
+### Camera-lidar calibration
+
+- [Extrinsic interactive camera-lidar calibration](docs/how_to_extrinsic_interactive.md)
+
+To calibrate camera-lidar pairs of sensors, a common practice is to compute corresponding pairs of points and then minimize their reprojection error. This calibration method implements a UI to help the selection of these pairs and evaluate their performance interactively.
+
+<figure align="center">
+   <img src="docs/images/camera-lidar/interactive_calibration_ui.png" width="400">
+   <figcaption align="center">Fig 5. Interactive camera-lidar calibration UI</figcaption>
+</figure>
+
+- [Tag-based camera-lidar calibration](docs/how_to_extrinsic_tag_based.md)
+
+This calibration method extends the interactive calibration by performing the corresponding points acquisition automatically via moving a known tag (lidartag) through the shared field of view.
+
+<figure align="center">
+   <img src="docs/images/camera-lidar/correct_calibration.png" width="400">
+   <figcaption align="center">Fig 6. Tag-based camera-lidar calibration</figcaption>
+</figure>
 
 ## Intrinsic Calibration
 
-- [How to Camera Calibration](docs/how_to_camera.md)
+Intrinsic calibration is the process of obtaining the parameters that allow us to transform raw sensor information into a coordinate system. In the case of cameras, it refers to the camera matrix and the distortion parameters, whereas in lidars it can refer to the offset and spacing of beams. In our repository, we focus only on camera calibration, since lidar intrinsic calibration is usually vendor-specific (and either the direct parameters or instructions are usually provided by them).
+
+### Intrinsic camera calibration
+
+- [Camera calibration](docs/how_to_camera.md)
+
+Based on the traditional ROS camera calibration procedure, we add extra visualizations to aid the calibration process and support for apriltag boards.
+
+- [Camera calibration via camera-lidar calibration](docs/how_to_extrinsic_interactive.md)
 
-- [How to Camera Calibration via camera-lidar calibration (Experimental)](docs/how_to_extrinsic_interactive.md)
+Traditionally, camera calibration is performed by detecting several views of planar boards in an image, and so, by knowing the board's dimensions, the camera matrix and distortion can be computed. However, this same process can also be performed by obtaining generic camera-object pairs, like the ones obtained during camera-lidar calibration. By reutilizing the calibration points from camera-lidar calibration, we can perform camera intrinsic calibration and camera-lidar extrinsic calibration simultaneously.
diff --git a/sensor/docs/design/sensor_calibration.svg b/sensor/docs/design/sensor_calibration.svg
new file mode 100644
index 00000000..7df30e53
--- /dev/null
+++ b/sensor/docs/design/sensor_calibration.svg
@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="662px" height="322px" viewBox="-0.5 -0.5 662 322" content="&lt;mxfile host=&quot;app.diagrams.net&quot; modified=&quot;2022-07-14T03:53:48.368Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/98.0.4758.80 Safari/537.36&quot; etag=&quot;JXK0tR0NDSuCZQaMxgBv&quot; version=&quot;20.0.4&quot; type=&quot;google&quot;&gt;&lt;diagram id=&quot;5hCaTmQIupewZZc91yCJ&quot; name=&quot;Page-1&quot;&gt;7ZrNctsgEICfxjPtIR1JWLJ6TJy4PTTTTtxOk14yWMISDRYehGI7T19koV8k106kStPkkogFFtj9gAU8AtPV9hODa/+auoiMDM3djsDlyDB0YGriXyzZJZLJRE8EHsOuLJQL5vgJSaGs50XYRWGpIKeUcLwuCx0aBMjhJRlkjG7KxZaUlFtdQw8pgrkDiSr9iV3uJ1I7HVYs/4yw56ct65rMWcG0sBSEPnTppiACVyMwZZTy5Gu1nSISGy+1S1Jv1pCbdYyhgB9TAcy9WzRf/Ihm59dXU+vXeAZuziaJlkdIIjlg2Vm+Sy2AXGEQmaSM+9SjASRXufSC0ShwUdyMJlJ5mS+UroVQF8LfiPOd9C6MOBUin6+IzE3ajBtqHJsUhTRiDjowoJQRyDzED5QzMg8IdBFdIc52oh5DBHL8WO4HlAx5WbnczOJDWvoEq+uK1RcwRPcEBw+q+QkRrMdm3viYo/ka7se/EdOtzoiPiHG0PWxGddiywliiKueqLZObHHw9LeIXoE9lrdvJ/t/oNI6kE/RJp6FYPURBSNn9A+b3wwA1Q3MopOqq0fpA1YWhv68fJ5aYkCkllO2bB66JbHcs5CFn9AEVcmxjASwry0n3OaNd9sGR7I/7ZB8obiTYhUwbCPemMTDux/X20gdqL0Pr22CmYjAHrhCDor1Z8jU8qwGjb6t9PGJ1DdzzON4XKYfAMMRO2S7ltVVYgu1ui4m7OPHBTJOX22Lm5S5NbTG/lRrj70ItkcorxYm0Tg/xQ8MaWvCfWeO+VHb0Uitb+EaxGEghiqzgM6lgkQxT1iqeWKqKzAYOU0WJHRRFe8SyYb9gU9fesDsJO/MNuzawU4+HCnZOxB6zQO9EBiVLOT93RXxqWcq5zVG9K5Faz+3BeBQie+nUxaOWY6PFsut49Nibgoaz2CuBWoAFd4Vi67hA2NxhvXIyG2sHu1UNNcrFxUfSfrvzSw3yu5hfp6zV2fzST9sXDs2v5RJZTu38cicfF5o2lPNer5uGbpsl/kzwzPllWX9R1NL8mlQWBN06PMGq5dMjY7czTD3lqIHTcC7uXnDZ0EBV07HzVKqMqveqiroORaxBOLK3MKJ7MqpH5meTUVXU1n7e0OFu149hPEv1dpvaPXZ2W9hVFbWFHehj2xrGe1NvQV3n2CmHjudi19npZVzfTmO/GgbYLabqbejN13ks+T4Tf945kOAFE36jgUjSiK8j/l4BmaMtL3MnqnmB+HYEFUhQdRFfImOh7VxmrLDrJoijED/BxV5VjJw0q9BrXozMy1iXoDpMANcVWgMaoAraUiQH9qKnQWNSckkW4Rcvr+2aE0U1YG/vPbX+uf9s/1Ii5CV/vRYvVdd3o8ZL4J96yVC8tHfQm5uKbrJ6d5N6eZM8m71uPxl2eR/SavxUd43yDD+JZP6jtWRPy3/6B67+AA==&lt;/diagram&gt;&lt;/mxfile&gt;"><defs/><g><path d="M 140 61 L 233.63 61" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 238.88 61 L 231.88 64.5 L 233.63 61 L 231.88 57.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/><ellipse cx="70" cy="61" rx="70" ry="20" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 61px; margin-left: 1px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">base_link</div></div></div></foreignObject><text x="70" y="65" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">base_link</text></switch></g><path d="M 380 61 L 473.63 61" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 478.88 61 L 471.88 64.5 L 473.63 61 L 471.88 57.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/><ellipse cx="310" cy="61" rx="70" ry="20" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 61px; margin-left: 241px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">sensor_kit_base_link</div></div></div></foreignObject><text x="310" y="65" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">sensor_kit_base_link</text></switch></g><path d="M 550 81 L 550 152.76" fill="none" stroke="#82b366" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 550 158.76 L 546 150.76 L 550 152.76 L 554 150.76 Z" fill="#82b366" stroke="#82b366" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><ellipse cx="550" cy="61" rx="70" ry="20" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 61px; margin-left: 481px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">lidar0_base_link</div></div></div></foreignObject><text x="550" y="65" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">lidar0_base_link</text></switch></g><ellipse cx="550" cy="181" rx="70" ry="20" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 181px; margin-left: 481px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">lidar1_base_link</div></div></div></foreignObject><text x="550" y="185" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">lidar1_base_link</text></switch></g><ellipse cx="550" cy="301" rx="70" ry="20" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 301px; margin-left: 481px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">camera0/camera_link</div></div></div></foreignObject><text x="550" y="305" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">camera0/camera_link</text></switch></g><path d="M 380 61 L 475.92 176.11" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 479.28 180.14 L 472.11 177 L 475.92 176.11 L 477.49 172.52 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/><path d="M 380 61 L 477.55 295.12" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 479.57 299.97 L 473.65 294.85 L 477.55 295.12 L 480.11 292.16 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/><path d="M 70 41 Q 140 1 310 1 Q 480 1 542.85 36.91" fill="none" stroke="#6c8ebf" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 548.06 39.89 L 539.13 39.39 L 542.85 36.91 L 543.1 32.45 Z" fill="#6c8ebf" stroke="#6c8ebf" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 620 61 Q 660 121 660 181 Q 660 241 624.57 294.15" fill="none" stroke="#d79b00" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 621.24 299.14 L 622.35 290.26 L 624.57 294.15 L 629.01 294.7 Z" fill="#d79b00" stroke="#d79b00" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 80 161 L 153.63 161" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 158.88 161 L 151.88 164.5 L 153.63 161 L 151.88 157.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/><path d="M 80 201 L 151.76 201" fill="none" stroke="#6c8ebf" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 157.76 201 L 149.76 205 L 151.76 201 L 149.76 197 Z" fill="#6c8ebf" stroke="#6c8ebf" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 80 241 L 90 241 L 151.76 241" fill="none" stroke="#82b366" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 157.76 241 L 149.76 245 L 151.76 241 L 149.76 237 Z" fill="#82b366" stroke="#82b366" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 80 281 L 100 281 L 151.76 281" fill="none" stroke="#d79b00" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 157.76 281 L 149.76 285 L 151.76 281 L 149.76 277 Z" fill="#d79b00" stroke="#d79b00" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><rect x="187" y="146" width="180" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 161px; margin-left: 277px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: nowrap;">ROS2 TF (calibration output)</div></div></div></foreignObject><text x="277" y="165" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">ROS2 TF (calibration output)</text></switch></g><rect x="190" y="186" width="130" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 201px; margin-left: 255px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: nowrap;">base-lidar calibration</div></div></div></foreignObject><text x="255" y="205" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">base-lidar calibration</text></switch></g><rect x="190" y="226" width="130" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 241px; margin-left: 255px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: nowrap;">lidar-lidar calibration</div></div></div></foreignObject><text x="255" y="245" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">lidar-lidar calibrati...</text></switch></g><rect x="188" y="266" width="150" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 281px; margin-left: 263px;"><div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;"><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: nowrap;">camera-lidar calibration</div></div></div></foreignObject><text x="263" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">camera-lidar calibration</text></switch></g></g><switch><g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/><a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank"><text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text></a></switch></svg>
\ No newline at end of file
diff --git a/sensor/docs/how_to_extrinsic_auto.md b/sensor/docs/how_to_extrinsic_map_based.md
similarity index 98%
rename from sensor/docs/how_to_extrinsic_auto.md
rename to sensor/docs/how_to_extrinsic_map_based.md
index 1ff758d1..debe2c9a 100644
--- a/sensor/docs/how_to_extrinsic_auto.md
+++ b/sensor/docs/how_to_extrinsic_map_based.md
@@ -99,7 +99,7 @@ For example,
 
 ```sh
 ros2 launch extrinsic_calibration_manager calibration.launch.xml \
-  mode:=manual sensor_model:=aip_xx1 vehicle_model:=jpntaxi map_path:=$HOME/map pointcloud_map_file:=0.20_map_clean.pcd lanelet2_map_file:=r1.osm
+  mode:=map_based sensor_model:=aip_xx1 vehicle_model:=jpntaxi map_path:=$HOME/map pointcloud_map_file:=0.20_map_clean.pcd lanelet2_map_file:=r1.osm
 ```
 
 Play your rosbag (on terminal 2).

From 63c5f81ce9380c482853e156ab0af042a354eb52 Mon Sep 17 00:00:00 2001
From: yabuta <makoto.yabuta@tier4.jp>
Date: Tue, 19 Jul 2022 10:44:34 +0900
Subject: [PATCH 16/20] add COLCON_IGNORE

---
 control/stop_accel_evaluator/COLCON_IGNORE | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 control/stop_accel_evaluator/COLCON_IGNORE

diff --git a/control/stop_accel_evaluator/COLCON_IGNORE b/control/stop_accel_evaluator/COLCON_IGNORE
new file mode 100644
index 00000000..e69de29b

From baf87d08092ec5c2e4c020043b84ae7b65c449f8 Mon Sep 17 00:00:00 2001
From: yabuta <makoto.yabuta@tier4.jp>
Date: Tue, 19 Jul 2022 18:57:51 +0900
Subject: [PATCH 17/20] add action

---
 .github/workflows/sync-develop-to-humble.yml | 27 ++++++++++++++++++++
 1 file changed, 27 insertions(+)
 create mode 100644 .github/workflows/sync-develop-to-humble.yml

diff --git a/.github/workflows/sync-develop-to-humble.yml b/.github/workflows/sync-develop-to-humble.yml
new file mode 100644
index 00000000..a7aa9316
--- /dev/null
+++ b/.github/workflows/sync-develop-to-humble.yml
@@ -0,0 +1,27 @@
+name: sync-develop-to-humble
+on:
+  schedule:
+    - cron: 0 0 * * *
+  workflow_dispatch:
+
+jobs:
+  sync-branches:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v1
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Run sync-branches
+        uses: autowarefoundation/autoware-github-actions/sync-branches@v1
+        with:
+          token: ${{ steps.generate-token.outputs.token }}
+          base-branch: humble
+          sync-target-repository: git@github.com:tier4/CalibrationTools.git
+          sync-target-branch: develop
+          sync-branch: sync-develop-to-humble
+          pr-title: "chore: sync develop to humble"
+          auto-merge-method: merge

From ff3a21db9f49cf2f4b7ff84194d01eea6b9fd62f Mon Sep 17 00:00:00 2001
From: yabuta <makoto.yabuta@tier4.jp>
Date: Tue, 19 Jul 2022 18:58:26 +0900
Subject: [PATCH 18/20] pre commit

---
 .github/PULL_REQUEST_TEMPLATE.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md
index 5a48d410..2df18b2b 100644
--- a/.github/PULL_REQUEST_TEMPLATE.md
+++ b/.github/PULL_REQUEST_TEMPLATE.md
@@ -39,4 +39,4 @@ The PR author **must** check the checkboxes below before merging.
 After all checkboxes are checked, anyone who has write access can merge the PR.
 
 [contribution guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/
-[pull request guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/
\ No newline at end of file
+[pull request guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/

From 5c5fbd0b3d6ce8a3a523ad5a223b2b7e310ea7ad Mon Sep 17 00:00:00 2001
From: yabuta <makoto.yabuta@tier4.jp>
Date: Wed, 20 Jul 2022 16:39:10 +0900
Subject: [PATCH 19/20] fix

---
 ...op-to-humble.yml => sync-tier4-universe-to-humble.yml} | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)
 rename .github/workflows/{sync-develop-to-humble.yml => sync-tier4-universe-to-humble.yml} (76%)

diff --git a/.github/workflows/sync-develop-to-humble.yml b/.github/workflows/sync-tier4-universe-to-humble.yml
similarity index 76%
rename from .github/workflows/sync-develop-to-humble.yml
rename to .github/workflows/sync-tier4-universe-to-humble.yml
index a7aa9316..f4f5aa4d 100644
--- a/.github/workflows/sync-develop-to-humble.yml
+++ b/.github/workflows/sync-tier4-universe-to-humble.yml
@@ -1,4 +1,4 @@
-name: sync-develop-to-humble
+name: sync-tier4-universe-to-humble
 on:
   schedule:
     - cron: 0 0 * * *
@@ -21,7 +21,7 @@ jobs:
           token: ${{ steps.generate-token.outputs.token }}
           base-branch: humble
           sync-target-repository: git@github.com:tier4/CalibrationTools.git
-          sync-target-branch: develop
-          sync-branch: sync-develop-to-humble
-          pr-title: "chore: sync develop to humble"
+          sync-target-branch: tier4/universe
+          sync-branch: sync-tier4-universe-to-humble
+          pr-title: "chore: sync tier4/universe to humble"
           auto-merge-method: merge

From 4a57bf08ed4a61b40e351055b6eda2f72b286aee Mon Sep 17 00:00:00 2001
From: Yohei Mishina <66298900+YoheiMishina@users.noreply.github.com>
Date: Wed, 27 Jul 2022 11:59:59 +0900
Subject: [PATCH 20/20] feat(map based calibrator): added 6dig search (#10)

* Modified map based calibration for lidar to lidar

* Deleted unnecessary file

* fix cmakelist and launch for PR

* Update sensor/extrinsic_calibration_manager/src/extrinsic_calibration_manager_node.cpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update sensor/extrinsic_calibration_manager/include/extrinsic_calibration_manager/extrinsic_calibration_manager_node.hpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_calibrator.cpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_calibrator.cpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_calibrator.cpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_calibrator.hpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_calibrator.hpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_calibrator.cpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_preprocessing.hpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_preprocessing.hpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/grid_search_matching.hpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/grid_search_matching.hpp

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Apply suggestions from code review

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* fix for parallel processing optimization

* fix for build error

* Modified calibration param for xx1

* added threshold param to calibration manager

* using localization for sensor calibration with logging sim

* Modified judgement timing in extrinsic calib manager

* Chage default threshold for calibration manager

* Moved rviz calls to map based calib.launch

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
---
 sensor/docs/how_to_extrinsic_map_based.md     |    6 +-
 .../extrinsic_calibration_manager_node.hpp    |    1 +
 .../launch/aip_x1/map_based.launch.xml        |   18 +-
 .../aip_x1/map_based_sensor_kit.launch.xml    |   30 +-
 .../launch/aip_xx1/map_based.launch.xml       |   39 +
 .../aip_xx1/map_based_sensor_kit.launch.xml   |   72 +
 .../aip_xx1/map_based_sensors.launch.xml      |   51 +
 .../launch/calibration.launch.xml             |    8 +-
 .../extrinsic_calibration_manager_node.cpp    |   20 +-
 .../CMakeLists.txt                            |   17 +
 .../aip_x1/extrinsic_map_based_calib.yaml     |   40 +
 .../config/aip_x1/map_based_calib.rviz        | 1978 ++++++++++++++++
 .../aip_x2/extrinsic_map_based_calib.yaml     |   31 +
 .../config/aip_x2/map_based_calib.rviz        | 1952 ++++++++++++++++
 .../aip_xx1/extrinsic_map_based_calib.yaml    |   40 +
 .../config/aip_xx1/map_based_calib.rviz       | 2058 +++++++++++++++++
 .../config/extrinsic_map_based_calib.yaml     |    9 +
 .../extrinsic_map_based_calibrator.hpp        |   10 +-
 .../extrinsic_map_based_preprocessing.hpp     |   12 +-
 .../grid_search_matching.hpp                  |   13 +-
 .../launch/calibrator.launch.xml              |    5 +-
 .../src/extrinsic_map_based_calibrator.cpp    |  164 +-
 .../src/extrinsic_map_based_preprocessing.cpp |   23 +-
 .../src/grid_search_matching.cpp              |  104 +-
 24 files changed, 6575 insertions(+), 126 deletions(-)
 create mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based.launch.xml
 create mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensor_kit.launch.xml
 create mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensors.launch.xml
 create mode 100644 sensor/extrinsic_map_based_calibrator/config/aip_x1/extrinsic_map_based_calib.yaml
 create mode 100644 sensor/extrinsic_map_based_calibrator/config/aip_x1/map_based_calib.rviz
 create mode 100644 sensor/extrinsic_map_based_calibrator/config/aip_x2/extrinsic_map_based_calib.yaml
 create mode 100644 sensor/extrinsic_map_based_calibrator/config/aip_x2/map_based_calib.rviz
 create mode 100644 sensor/extrinsic_map_based_calibrator/config/aip_xx1/extrinsic_map_based_calib.yaml
 create mode 100644 sensor/extrinsic_map_based_calibrator/config/aip_xx1/map_based_calib.rviz

diff --git a/sensor/docs/how_to_extrinsic_map_based.md b/sensor/docs/how_to_extrinsic_map_based.md
index debe2c9a..6cba0070 100644
--- a/sensor/docs/how_to_extrinsic_map_based.md
+++ b/sensor/docs/how_to_extrinsic_map_based.md
@@ -102,12 +102,16 @@ ros2 launch extrinsic_calibration_manager calibration.launch.xml \
   mode:=map_based sensor_model:=aip_xx1 vehicle_model:=jpntaxi map_path:=$HOME/map pointcloud_map_file:=0.20_map_clean.pcd lanelet2_map_file:=r1.osm
 ```
 
+The previous commands are used when calibration is performed with loss bag data. However, it is also possible to calibrate with actual vehicles, in which case the logging_simulator and related functions are not needed, so the command needs to be modified. In this case, add logging_simulator:=false
+
 Play your rosbag (on terminal 2).
 
 ```sh
-ros2 bag play <rosbag_path> --clock 100 -l -r 0.1
+ros2 bag play <rosbag_path> --clock 100 -l -r 0.1  --remap /tf:=/null/tf /tf_static:=/null/tf_static
 ```
 
+The TFs required for calibration are generated by the logging simulator, so the rosbag TFs are remap.
+
 Now you are ready to start.
 
 Then, when the system completes the calibration, rviz will display the calibrated point cloud and the console will display the score.
diff --git a/sensor/extrinsic_calibration_manager/include/extrinsic_calibration_manager/extrinsic_calibration_manager_node.hpp b/sensor/extrinsic_calibration_manager/include/extrinsic_calibration_manager/extrinsic_calibration_manager_node.hpp
index db910ba0..41eca01a 100644
--- a/sensor/extrinsic_calibration_manager/include/extrinsic_calibration_manager/extrinsic_calibration_manager_node.hpp
+++ b/sensor/extrinsic_calibration_manager/include/extrinsic_calibration_manager/extrinsic_calibration_manager_node.hpp
@@ -69,6 +69,7 @@ class ExtrinsicCalibrationManagerNode : public rclcpp::Node
   std::string parent_frame_;
   std::vector<std::string> child_frames_;
   std::string client_ns_;
+  double threshold_;
 
   void calibrationRequestCallback(
     const std::shared_ptr<tier4_calibration_msgs::srv::ExtrinsicCalibrationManager::Request>
diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based.launch.xml
index 8e8019fe..ea0b3ae0 100644
--- a/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based.launch.xml
+++ b/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based.launch.xml
@@ -4,16 +4,28 @@
   <arg name="vehicle_id" default="default" />
   <arg name="map_path" default="(env HOME)/map/" />
   <arg name="map_calibration_area_file" default="map_calibration_area.pcd"/>
+  <arg name="use_calibration_area_map" default="true" />
+  <arg name="calibration_rviz" default="true"/>
 
   <let name="sensor_model" value="aip_x1" />
 
+  <!-- rviz -->
+  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="
+        -d $(find-pkg-share extrinsic_map_based_calibrator)/config/$(var sensor_model)/map_based_calib.rviz" if="$(var calibration_rviz)" />
+
   <group>
     <push-ros-namespace namespace="sensor_kit" />
     <include file="$(find-pkg-share extrinsic_calibration_manager)/launch/$(var sensor_model)/map_based_sensor_kit.launch.xml">
       <arg name="vehicle_id" value="$(var vehicle_id)" />
-      <arg name="map_path" value="$(var map_path)" />
-      <arg name="pointcloud_map_file" value="$(var pointcloud_map_file)" />
-      <arg name="map_calibration_area_file" value="$(var map_calibration_area_file)" />
+      <arg name="use_calibration_area_map" value="$(var use_calibration_area_map)" />
+    </include>
+  </group>
+
+  <!-- Map -->
+  <group if="$(var use_calibration_area_map)">
+    <push-ros-namespace namespace="sensor_kit" />
+    <include file="$(find-pkg-share extrinsic_map_based_calibrator)/launch/map.launch.py">
+      <arg name="pointcloud_map_path" value="$(var map_path)/$(var map_calibration_area_file)"/>
     </include>
   </group>
 
diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based_sensor_kit.launch.xml
index 683d66be..c940b7e8 100644
--- a/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based_sensor_kit.launch.xml
+++ b/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based_sensor_kit.launch.xml
@@ -2,13 +2,13 @@
 <launch>
 
   <arg name="vehicle_id" default="default" />
-  <arg name="map_path" default="$(env HOME)/map" />
-  <arg name="map_calibration_area_file" default="map_calibration_area.pcd"/>
   <let name="sensor_model" value="aip_x1" />
   <let name="parent_frame" value="sensor_kit_base_link" />
   <let name="input_map_with_wall_topic_name" value="/map/pointcloud_map" />
   <let name="input_map_without_wall_topic_name" value="/map/pointcloud_map_without_wall" />
-  <arg name="accumulate_frame_num" default="10" />
+  <arg name="use_calibration_area_map" default="true" />
+  <arg name="accumulate_frame_num" default="5" />
+  <arg name="fitness_score_threshold" default="0.001" />
   <arg name="config_file_name" default="extrinsic_map_based_calib.yaml" />
 
   <!-- extrinsic_calibration_client -->
@@ -27,19 +27,10 @@
     [livox_front_left_base_link,
     livox_front_center_base_link,
     livox_front_right_base_link]" />
+    <param name="fitness_score_threshold" value="$(var fitness_score_threshold)" />
   </node>
 
   <!-- map based calibrator -->
-  <!-- <include file="$(find-pkg-share extrinsic_map_based_calibrator)/launch/calibrator.launch.xml">
-    <arg name="ns" value="$(var parent_frame)/velodyne_top_base_link" />
-    <arg name="parent_frame" value="$(var parent_frame)" />
-    <arg name="child_frame" value="velodyne_top_base_link" />
-    <arg name="input_map_with_wall_topic_name" value="$(var input_map_with_wall_topic_name)" />
-    <arg name="input_map_without_wall_topic_name" value="$(var input_map_without_wall_topic_name)" />
-    <arg name="input_source_topic_name" value="/sensing/lidar/top/pointcloud_raw" />
-    <arg name="is_debug_pub" value="$(var is_debug_pub)" />
-  </include> -->
-
   <group>
     <include file="$(find-pkg-share point_cloud_accumulator)/launch/point_cloud_accumulator.launch.xml">
       <arg name="ns" value="$(var parent_frame)/livox_front_left_base_link" />
@@ -55,6 +46,8 @@
       <arg name="input_map_with_wall_topic_name" value="$(var input_map_with_wall_topic_name)" />
       <arg name="input_map_without_wall_topic_name" value="$(var input_map_without_wall_topic_name)" />
       <arg name="input_source_topic_name" value="/sensing/lidar/front_left/livox/accumulated_pointcloud" />
+      <arg name="use_calibration_area_map" value="$(var use_calibration_area_map)" />
+      <arg name="sensor_model" value="$(var sensor_model)" />
     </include>
   </group>
 
@@ -73,6 +66,8 @@
       <arg name="input_map_with_wall_topic_name" value="$(var input_map_with_wall_topic_name)" />
       <arg name="input_map_without_wall_topic_name" value="$(var input_map_without_wall_topic_name)" />
       <arg name="input_source_topic_name" value="/sensing/lidar/front_center/livox/accumulated_pointcloud" />
+      <arg name="use_calibration_area_map" value="$(var use_calibration_area_map)" />
+      <arg name="sensor_model" value="$(var sensor_model)" />
     </include>
   </group>
 
@@ -91,13 +86,8 @@
       <arg name="input_map_with_wall_topic_name" value="$(var input_map_with_wall_topic_name)" />
       <arg name="input_map_without_wall_topic_name" value="$(var input_map_without_wall_topic_name)" />
       <arg name="input_source_topic_name" value="/sensing/lidar/front_right/livox/accumulated_pointcloud" />
-    </include>
-  </group>
-
-  <!-- Map -->
-  <group>
-    <include file="$(find-pkg-share extrinsic_map_based_calibrator)/launch/map.launch.py">
-      <arg name="pointcloud_map_path" value="$(var map_path)/$(var map_calibration_area_file)"/>
+      <arg name="use_calibration_area_map" value="$(var use_calibration_area_map)" />
+      <arg name="sensor_model" value="$(var sensor_model)" />
     </include>
   </group>
 
diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based.launch.xml
new file mode 100644
index 00000000..cf6ac666
--- /dev/null
+++ b/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based.launch.xml
@@ -0,0 +1,39 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+  <arg name="vehicle_id" default="default" />
+  <arg name="map_path" default="(env HOME)/map/" />
+  <arg name="map_calibration_area_file" default="map_calibration_area.pcd"/>
+  <arg name="use_calibration_area_map" default="true" />
+  <arg name="calibration_rviz" default="true"/>
+  <let name="sensor_model" value="aip_xx1" />
+
+  <!-- rviz -->
+  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="
+        -d $(find-pkg-share extrinsic_map_based_calibrator)/config/$(var sensor_model)/map_based_calib.rviz" if="$(var calibration_rviz)" />
+
+  <group>
+    <push-ros-namespace namespace="sensor_kit" />
+    <include file="$(find-pkg-share extrinsic_calibration_manager)/launch/$(var sensor_model)/map_based_sensor_kit.launch.xml">
+      <arg name="vehicle_id" value="$(var vehicle_id)" />
+      <arg name="use_calibration_area_map" value="$(var use_calibration_area_map)" />
+    </include>
+  </group>
+
+  <group>
+    <push-ros-namespace namespace="sensors" />
+    <include file="$(find-pkg-share extrinsic_calibration_manager)/launch/$(var sensor_model)/map_based_sensors.launch.xml">
+      <arg name="vehicle_id" value="$(var vehicle_id)" />
+      <arg name="use_calibration_area_map" value="$(var use_calibration_area_map)" />
+    </include>
+  </group>
+
+  <!-- Map -->
+  <group if="$(var use_calibration_area_map)">
+    <push-ros-namespace namespace="sensor_kit" />
+    <include file="$(find-pkg-share extrinsic_map_based_calibrator)/launch/map.launch.py">
+      <arg name="pointcloud_map_path" value="$(var map_path)/$(var map_calibration_area_file)"/>
+    </include>
+  </group>
+
+</launch>
diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensor_kit.launch.xml
new file mode 100644
index 00000000..bb6272bd
--- /dev/null
+++ b/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensor_kit.launch.xml
@@ -0,0 +1,72 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+  <arg name="vehicle_id" default="default" />
+  <let name="sensor_model" value="aip_xx1" />
+  <let name="parent_frame" value="sensor_kit_base_link" />
+  <let name="input_map_with_wall_topic_name" value="/map/pointcloud_map" />
+  <let name="input_map_without_wall_topic_name" value="/map/pointcloud_map_without_wall" />
+  <arg name="use_calibration_area_map" default="true" />
+  <arg name="accumulate_frame_num" default="2" />
+  <arg name="fitness_score_threshold" default="0.001" />
+  <arg name="config_file_name" default="extrinsic_map_based_calib.yaml" />
+
+  <!-- extrinsic_calibration_client -->
+  <arg name="src_yaml" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/$(var sensor_model)/sensor_kit_calibration.yaml" />
+  <arg name="dst_yaml" default="$(env HOME)/sensor_kit_calibration.yaml" />
+
+  <node pkg="extrinsic_calibration_client" exec="extrinsic_calibration_client" name="extrinsic_calibration_client" output="screen">
+    <param name="src_path" value="$(var src_yaml)" />
+    <param name="dst_path" value="$(var dst_yaml)" />
+  </node>
+
+  <!-- extrinsic_calibration_manager -->
+  <node pkg="extrinsic_calibration_manager" exec="extrinsic_calibration_manager" name="extrinsic_calibration_manager" output="screen">
+    <param name="parent_frame" value="$(var parent_frame)" />
+    <param name="child_frames" value="
+    [velodyne_left_base_link,
+    velodyne_right_base_link]" />
+    <param name="fitness_score_threshold" value="$(var fitness_score_threshold)" />
+  </node>
+
+  <group>
+    <include file="$(find-pkg-share point_cloud_accumulator)/launch/point_cloud_accumulator.launch.xml">
+      <arg name="ns" value="$(var parent_frame)/velodyne_left_base_link" />
+      <arg name="input_sensor_topic_name" value="/sensing/lidar/left/pointcloud_raw" />
+      <arg name="output_sensor_topic_name" value="/sensing/lidar/left/pointcloud_raw/accumulated_pointcloud" />
+      <arg name="accumulate_frame_num" value="$(var accumulate_frame_num)" />
+    </include>
+    <include file="$(find-pkg-share extrinsic_map_based_calibrator)/launch/calibrator.launch.xml">
+      <arg name="ns" value="$(var parent_frame)/velodyne_left_base_link" />
+      <arg name="parent_frame" value="$(var parent_frame)" />
+      <arg name="child_frame" value="velodyne_left_base_link" />
+      <arg name="config_file_name" value="$(var config_file_name)" />
+      <arg name="input_map_with_wall_topic_name" value="$(var input_map_with_wall_topic_name)" />
+      <arg name="input_map_without_wall_topic_name" value="$(var input_map_without_wall_topic_name)" />
+      <arg name="input_source_topic_name" value="/sensing/lidar/left/pointcloud_raw/accumulated_pointcloud" />
+      <arg name="use_calibration_area_map" value="$(var use_calibration_area_map)" />
+      <arg name="sensor_model" value="$(var sensor_model)" />
+    </include>
+  </group>
+
+  <group>
+    <include file="$(find-pkg-share point_cloud_accumulator)/launch/point_cloud_accumulator.launch.xml">
+      <arg name="ns" value="$(var parent_frame)/velodyne_right_base_link" />
+      <arg name="input_sensor_topic_name" value="/sensing/lidar/right/pointcloud_raw" />
+      <arg name="output_sensor_topic_name" value="/sensing/lidar/right/pointcloud_raw/accumulated_pointcloud" />
+      <arg name="accumulate_frame_num" value="$(var accumulate_frame_num)" />
+    </include>
+    <include file="$(find-pkg-share extrinsic_map_based_calibrator)/launch/calibrator.launch.xml">
+      <arg name="ns" value="$(var parent_frame)/velodyne_right_base_link" />
+      <arg name="parent_frame" value="$(var parent_frame)" />
+      <arg name="child_frame" value="velodyne_right_base_link" />
+      <arg name="config_file_name" value="$(var config_file_name)" />
+      <arg name="input_map_with_wall_topic_name" value="$(var input_map_with_wall_topic_name)" />
+      <arg name="input_map_without_wall_topic_name" value="$(var input_map_without_wall_topic_name)" />
+      <arg name="input_source_topic_name" value="/sensing/lidar/right/pointcloud_raw/accumulated_pointcloud" />
+      <arg name="use_calibration_area_map" value="$(var use_calibration_area_map)" />
+      <arg name="sensor_model" value="$(var sensor_model)" />
+    </include>
+  </group>
+
+</launch>
diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensors.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensors.launch.xml
new file mode 100644
index 00000000..182db3e1
--- /dev/null
+++ b/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensors.launch.xml
@@ -0,0 +1,51 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+  <arg name="vehicle_id" default="default" />
+  <let name="sensor_model" value="aip_xx1" />
+  <let name="parent_frame" value="base_link" />
+  <let name="input_map_with_wall_topic_name" value="/map/pointcloud_map" />
+  <let name="input_map_without_wall_topic_name" value="/map/pointcloud_map_without_wall" />
+  <arg name="accumulate_frame_num" default="2" />
+  <arg name="fitness_score_threshold" default="0.001" />
+  <arg name="config_file_name" default="extrinsic_map_based_calib.yaml" />
+  <arg name="use_calibration_area_map" default="true" />
+
+  <!-- extrinsic_calibration_client -->
+  <arg name="src_yaml" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/$(var sensor_model)/sensors_calibration.yaml" />
+  <arg name="dst_yaml" default="$(env HOME)/sensors_calibration.yaml" />
+
+  <node pkg="extrinsic_calibration_client" exec="extrinsic_calibration_client" name="extrinsic_calibration_client" output="screen">
+    <param name="src_path" value="$(var src_yaml)" />
+    <param name="dst_path" value="$(var dst_yaml)" />
+  </node>
+
+  <!-- extrinsic_calibration_manager -->
+  <node pkg="extrinsic_calibration_manager" exec="extrinsic_calibration_manager" name="extrinsic_calibration_manager" output="screen">
+    <param name="parent_frame" value="$(var parent_frame)" />
+    <param name="child_frames" value="
+    [velodyne_rear_base_link]" />
+    <param name="fitness_score_threshold" value="$(var fitness_score_threshold)" />
+  </node>
+
+  <group>
+    <include file="$(find-pkg-share point_cloud_accumulator)/launch/point_cloud_accumulator.launch.xml">
+      <arg name="ns" value="$(var parent_frame)/velodyne_rear_base_link" />
+      <arg name="input_sensor_topic_name" value="/sensing/lidar/rear/pointcloud_raw" />
+      <arg name="output_sensor_topic_name" value="/sensing/lidar/rear/pointcloud_raw/accumulated_pointcloud" />
+      <arg name="accumulate_frame_num" value="$(var accumulate_frame_num)" />
+    </include>
+    <include file="$(find-pkg-share extrinsic_map_based_calibrator)/launch/calibrator.launch.xml">
+      <arg name="ns" value="$(var parent_frame)/velodyne_rear_base_link" />
+      <arg name="parent_frame" value="$(var parent_frame)" />
+      <arg name="child_frame" value="velodyne_rear_base_link" />
+      <arg name="config_file_name" value="$(var config_file_name)" />
+      <arg name="input_map_with_wall_topic_name" value="$(var input_map_with_wall_topic_name)" />
+      <arg name="input_map_without_wall_topic_name" value="$(var input_map_without_wall_topic_name)" />
+      <arg name="input_source_topic_name" value="/sensing/lidar/rear/pointcloud_raw/accumulated_pointcloud" />
+      <arg name="use_calibration_area_map" value="$(var use_calibration_area_map)" />
+      <arg name="sensor_model" value="$(var sensor_model)" />
+    </include>
+  </group>
+
+</launch>
diff --git a/sensor/extrinsic_calibration_manager/launch/calibration.launch.xml b/sensor/extrinsic_calibration_manager/launch/calibration.launch.xml
index 430eb2ed..ac71f5e3 100644
--- a/sensor/extrinsic_calibration_manager/launch/calibration.launch.xml
+++ b/sensor/extrinsic_calibration_manager/launch/calibration.launch.xml
@@ -10,6 +10,7 @@
   <arg name="pointcloud_map_file" default="pointcloud_map.pcd"/>
   <arg name="logging_simulator" default="true"/>
   <arg name="map_calibration_area_file" default="map_calibration_area.pcd"/>
+  <arg name="use_calibration_area_map" default="true" />
 
   <!-- tag_based & interactive only -->
   <arg name="camera_name" default=""/> <!--e.g, camera6-->
@@ -31,7 +32,7 @@
     <arg name="system" value="false" />
     <arg name="map" value="true" />
     <arg name="sensing" value="true" />
-    <arg name="localization" value="false" />
+    <arg name="localization" value="true" />
     <arg name="perception" value="false" />
     <arg name="planning" value="false" />
     <arg name="control" value="false" />
@@ -56,11 +57,10 @@
       <include file="$(find-pkg-share extrinsic_calibration_manager)/launch/$(var sensor_model)/map_based.launch.xml">
         <arg name="vehicle_id" value="$(var vehicle_id)" />
         <arg name="map_path" value="$(var map_path)" />
-        <arg name="pointcloud_map_file" value="$(var pointcloud_map_file)" />
         <arg name="map_calibration_area_file" value="$(var map_calibration_area_file)" />
+        <arg name="use_calibration_area_map" value="$(var use_calibration_area_map)" />
+        <arg name="calibration_rviz" value="$(var calibration_rviz)" />
       </include>
-      <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="
-        -d $(find-pkg-share extrinsic_map_based_calibrator)/config/map_based_calib.rviz" if="$(var calibration_rviz)" />
     </group>
 
     <group if="$(eval &quot;'$(var mode)' == 'tag_based' &quot;)">
diff --git a/sensor/extrinsic_calibration_manager/src/extrinsic_calibration_manager_node.cpp b/sensor/extrinsic_calibration_manager/src/extrinsic_calibration_manager_node.cpp
index e03de4d3..19f02fc9 100644
--- a/sensor/extrinsic_calibration_manager/src/extrinsic_calibration_manager_node.cpp
+++ b/sensor/extrinsic_calibration_manager/src/extrinsic_calibration_manager_node.cpp
@@ -37,6 +37,7 @@ ExtrinsicCalibrationManagerNode::ExtrinsicCalibrationManagerNode(
   parent_frame_ = this->declare_parameter("parent_frame", "");
   child_frames_ = this->declare_parameter("child_frames", std::vector<std::string>());
   client_ns_ = this->declare_parameter("client_ns", "extrinsic_calibration");
+  threshold_ = this->declare_parameter("fitness_score_threshold", 10.0);
 }
 
 void ExtrinsicCalibrationManagerNode::calibrationRequestCallback(
@@ -110,9 +111,22 @@ void ExtrinsicCalibrationManagerNode::calibrationRequestCallback(
   // dump yaml file
   dumpCalibrationConfig(request->dst_path, target_clients_);
 
-  // TODO(Akihito OHSATO): handling results of success/score
-  response->success = true;
-  response->score = 0.0;
+  bool success = true;
+  double max_score = -std::numeric_limits<double>::max();
+  for (auto & target_client : target_clients_) {
+    if(!target_client.response.success){
+      success = false;
+    }
+    if( target_client.response.score > max_score ){
+      max_score = target_client.response.score;
+    }
+  }
+
+  if( max_score > threshold_ ){
+    success = false;
+  }
+  response->success = success;
+  response->score = max_score;
 }
 
 bool ExtrinsicCalibrationManagerNode::createTargetClient(
diff --git a/sensor/extrinsic_map_based_calibrator/CMakeLists.txt b/sensor/extrinsic_map_based_calibrator/CMakeLists.txt
index f1f110e8..a0f6ce15 100644
--- a/sensor/extrinsic_map_based_calibrator/CMakeLists.txt
+++ b/sensor/extrinsic_map_based_calibrator/CMakeLists.txt
@@ -4,12 +4,29 @@ project(extrinsic_map_based_calibrator)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+find_package(OpenMP REQUIRED)
+if(OpenMP_FOUND)
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+
 ament_auto_add_executable(extrinsic_map_based_calibrator
   src/extrinsic_map_based_calibrator.cpp
   src/extrinsic_map_based_preprocessing.cpp
   src/grid_search_matching.cpp)
 ament_target_dependencies(extrinsic_map_based_calibrator)
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+if(OpenMP_CXX_FOUND)
+  target_link_libraries(extrinsic_map_based_calibrator OpenMP::OpenMP_CXX)
+else()
+  message(WARNING "OpenMP not found")
+endif()
+
 ament_auto_package(
   INSTALL_TO_SHARE
     launch
diff --git a/sensor/extrinsic_map_based_calibrator/config/aip_x1/extrinsic_map_based_calib.yaml b/sensor/extrinsic_map_based_calibrator/config/aip_x1/extrinsic_map_based_calib.yaml
new file mode 100644
index 00000000..e9232e78
--- /dev/null
+++ b/sensor/extrinsic_map_based_calibrator/config/aip_x1/extrinsic_map_based_calib.yaml
@@ -0,0 +1,40 @@
+/**:
+  ros__parameters:
+    preprocessing:
+      ransac:
+        max_iteration: 1000
+        voxel_grid_size: 0.3
+        distance_threshold: 0.1
+      clip_wall_pointcloud:
+        max_iteration: 100
+        max_correspondence_distance: 100.0
+        transformation_epsilon: 1.0e-10
+        euclidean_fitness_epsilon: 0.001
+        clipping_threshold: 0.05
+
+    grid_search:
+      x_range_max: 0.05
+      x_range_min: -0.05
+      x_range_resolution: 0.025
+      y_range_max: 0.05
+      y_range_min: -0.05
+      y_range_resolution: 0.025
+      z_range_max: 0.05
+      z_range_min: -0.05
+      z_range_resolution: 0.25
+      roll_range_max: 1.0
+      roll_range_min: -1.0
+      roll_range_resolution: 0.25
+      pitch_range_max: 1.0
+      pitch_range_min: -1.0
+      pitch_range_resolution: 0.25
+      yaw_range_max: 1.0
+      yaw_range_min: -1.0
+      yaw_range_resolution: 0.25
+      maximum_iteration: 1000
+      max_correspondence_distance: 100.0
+      transformation_epsilon: 1.0e-10
+      euclidean_fitness_epsilon: 0.001
+
+    map_based_calibrator:
+      debug_pub: true
diff --git a/sensor/extrinsic_map_based_calibrator/config/aip_x1/map_based_calib.rviz b/sensor/extrinsic_map_based_calibrator/config/aip_x1/map_based_calib.rviz
new file mode 100644
index 00000000..43daed00
--- /dev/null
+++ b/sensor/extrinsic_map_based_calibrator/config/aip_x1/map_based_calib.rviz
@@ -0,0 +1,1978 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map1
+        - /Sensing1/LiDAR1
+        - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1
+        - /Center1
+        - /Left1
+        - /Right1
+      Splitter Ratio: 0.5829145908355713
+    Tree Height: 796
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded: ~
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel
+    Name: InitialPoseButtonPanel
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: map without wall
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/TF
+          Enabled: true
+          Frame Timeout: 15
+          Frames:
+            All Enabled: true
+            base_link:
+              Value: true
+            livox_front_center:
+              Value: true
+            livox_front_center_base_link:
+              Value: true
+            livox_front_left:
+              Value: true
+            livox_front_left_base_link:
+              Value: true
+            livox_front_right:
+              Value: true
+            livox_front_right_base_link:
+              Value: true
+            map:
+              Value: true
+            ndt_base_link:
+              Value: true
+            sensor_kit_base_link:
+              Value: true
+            velodyne_top:
+              Value: true
+            velodyne_top_base_link:
+              Value: true
+            viewer:
+              Value: true
+          Marker Scale: 1
+          Name: TF
+          Show Arrows: true
+          Show Axes: true
+          Show Names: true
+          Tree:
+            map:
+              base_link:
+                sensor_kit_base_link:
+                  livox_front_center_base_link:
+                    livox_front_center:
+                      {}
+                  livox_front_left_base_link:
+                    livox_front_left:
+                      {}
+                  livox_front_right_base_link:
+                    livox_front_right:
+                      {}
+                  velodyne_top_base_link:
+                    velodyne_top:
+                      {}
+              ndt_base_link:
+                {}
+              viewer:
+                {}
+          Update Interval: 0
+          Value: true
+        - Alpha: 0.5
+          Cell Size: 1
+          Class: rviz_default_plugins/Grid
+          Color: 160; 160; 164
+          Enabled: false
+          Line Style:
+            Line Width: 0.029999999329447746
+            Value: Lines
+          Name: Grid
+          Normal Cell Count: 0
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Plane: XY
+          Plane Cell Count: 10
+          Reference Frame: <Fixed Frame>
+          Value: false
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_plugins/SteeringAngle
+              Enabled: false
+              Left: 128
+              Length: 256
+              Name: SteeringAngle
+              Scale: 17
+              Text Color: 25; 255; 240
+              Top: 128
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /vehicle/status/steering
+              Value: false
+              Value Scale: 0.14999249577522278
+              Value height offset: 0
+            - Class: rviz_plugins/ConsoleMeter
+              Enabled: false
+              Left: 512
+              Length: 256
+              Name: ConsoleMeter
+              Text Color: 25; 255; 240
+              Top: 128
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /vehicle/status/twist
+              Value: false
+              Value Scale: 0.14999249577522278
+              Value height offset: 0
+            - Alpha: 0.9990000128746033
+              Class: rviz_plugins/VelocityHistory
+              Color Border Vel Max: 3
+              Constant Color:
+                Color: 255; 255; 255
+                Value: true
+              Enabled: true
+              Name: VelocityHistory
+              Scale: 0.30000001192092896
+              Timeout: 10
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /vehicle/status/twist
+              Value: true
+            - Alpha: 0.30000001192092896
+              Class: rviz_default_plugins/RobotModel
+              Collision Enabled: false
+              Description File: ""
+              Description Source: Topic
+              Description Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /robot_description
+              Enabled: true
+              Links:
+                All Links Enabled: true
+                Expand Joint Details: false
+                Expand Link Details: false
+                Expand Tree: false
+                Link Tree Style: Links in Alphabetic Order
+                base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                livox_front_center:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                livox_front_center_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                livox_front_left:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                livox_front_left_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                livox_front_right:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                livox_front_right_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                sensor_kit_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                velodyne_top:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                velodyne_top_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+              Name: VehicleModel
+              TF Prefix: ""
+              Update Interval: 0
+              Value: true
+              Visual Enabled: true
+            - Class: rviz_plugins/MaxVelocity
+              Enabled: true
+              Left: 595
+              Length: 96
+              Name: MaxVelocity
+              Text Color: 255; 255; 255
+              Top: 280
+              Topic: /planning/scenario_planning/current_max_velocity
+              Value: true
+              Value Scale: 0.25
+            - Class: rviz_plugins/TurnSignal
+              Enabled: false
+              Height: 256
+              Left: 196
+              Name: TurnSignal
+              Top: 350
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /control/turn_signal_cmd
+              Value: false
+              Width: 512
+          Enabled: true
+          Name: Vehicle
+      Enabled: true
+      Name: System
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 28.71826171875
+            Min Value: -7.4224700927734375
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 237
+          Min Color: 211; 215; 207
+          Min Intensity: 0
+          Name: PointCloudMap
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.019999999552965164
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /map/pointcloud_map
+          Use Fixed Frame: true
+          Use rainbow: false
+          Value: true
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 211; 215; 207
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: map without wall
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.019999999552965164
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /map/pointcloud_map_without_wall
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Lanelet2VectorMap
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /map/vector_map_marker
+          Value: false
+      Enabled: true
+      Name: Map
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Alpha: 0.4000000059604645
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 5
+                Min Value: -1
+                Value: false
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz_default_plugins/PointCloud2
+              Color: 255; 255; 255
+              Color Transformer: AxisColor
+              Decay Time: 0
+              Enabled: true
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: ConcatenatePointCloud
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 1
+              Size (m): 0.019999999552965164
+              Style: Points
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Best Effort
+                Value: /sensing/lidar/concatenated/pointcloud
+              Use Fixed Frame: false
+              Use rainbow: true
+              Value: true
+            - Alpha: 0.9990000128746033
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 15
+                Min Value: -2
+                Value: false
+              Axis: Z
+              Channel Name: z
+              Class: rviz_default_plugins/PointCloud2
+              Color: 200; 200; 200
+              Color Transformer: FlatColor
+              Decay Time: 0
+              Enabled: true
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 15
+              Min Color: 0; 0; 0
+              Min Intensity: -5
+              Name: NoGroundPointCloud
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 3
+              Size (m): 0.019999999552965164
+              Style: Points
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Best Effort
+                Value: /perception/obstacle_segmentation/pointcloud
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: true
+            - Alpha: 0.9990000128746033
+              Class: rviz_default_plugins/Polygon
+              Color: 25; 255; 0
+              Enabled: false
+              Name: MeasurementRange
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /sensing/lidar/crop_box_filter/crop_box_polygon
+              Value: false
+          Enabled: true
+          Name: LiDAR
+        - Class: rviz_common/Group
+          Displays:
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.10000000149011612
+              Class: rviz_default_plugins/PoseWithCovariance
+              Color: 233; 185; 110
+              Covariance:
+                Orientation:
+                  Alpha: 0.5
+                  Color: 255; 255; 127
+                  Color Style: Unique
+                  Frame: Local
+                  Offset: 1
+                  Scale: 1
+                  Value: false
+                Position:
+                  Alpha: 0.20000000298023224
+                  Color: 204; 51; 204
+                  Scale: 1
+                  Value: true
+                Value: true
+              Enabled: true
+              Head Length: 0.699999988079071
+              Head Radius: 1.2000000476837158
+              Name: PoseWithCovariance
+              Shaft Length: 1
+              Shaft Radius: 0.5
+              Shape: Arrow
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /sensing/gnss/pose_with_covariance
+              Value: true
+          Enabled: false
+          Name: GNSS
+      Enabled: false
+      Name: Sensing
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.10000000149011612
+              Class: rviz_default_plugins/PoseWithCovariance
+              Color: 0; 170; 255
+              Covariance:
+                Orientation:
+                  Alpha: 0.5
+                  Color: 255; 255; 127
+                  Color Style: Unique
+                  Frame: Local
+                  Offset: 1
+                  Scale: 1
+                  Value: true
+                Position:
+                  Alpha: 0.30000001192092896
+                  Color: 204; 51; 204
+                  Scale: 1
+                  Value: true
+                Value: true
+              Enabled: false
+              Head Length: 0.4000000059604645
+              Head Radius: 0.6000000238418579
+              Name: PoseWithCovInitial
+              Shaft Length: 0.6000000238418579
+              Shaft Radius: 0.30000001192092896
+              Shape: Arrow
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/initial_pose_with_covariance
+              Value: false
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.10000000149011612
+              Class: rviz_default_plugins/PoseWithCovariance
+              Color: 0; 255; 0
+              Covariance:
+                Orientation:
+                  Alpha: 0.5
+                  Color: 255; 255; 127
+                  Color Style: Unique
+                  Frame: Local
+                  Offset: 1
+                  Scale: 1
+                  Value: true
+                Position:
+                  Alpha: 0.30000001192092896
+                  Color: 204; 51; 204
+                  Scale: 1
+                  Value: true
+                Value: true
+              Enabled: false
+              Head Length: 0.4000000059604645
+              Head Radius: 0.6000000238418579
+              Name: PoseWithCovAligned
+              Shaft Length: 0.6000000238418579
+              Shaft Radius: 0.30000001192092896
+              Shape: Arrow
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/pose_with_covariance
+              Value: false
+            - Buffer Size: 200
+              Class: rviz_plugins::PoseHistory
+              Enabled: false
+              Line:
+                Alpha: 0.9990000128746033
+                Color: 170; 255; 127
+                Value: true
+                Width: 0.10000000149011612
+              Name: PoseHistory
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/pose
+              Value: false
+            - Alpha: 0.9990000128746033
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 10
+                Min Value: -10
+                Value: true
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz_default_plugins/PointCloud2
+              Color: 0; 255; 255
+              Color Transformer: ""
+              Decay Time: 0
+              Enabled: false
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: Initial
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 10
+              Size (m): 0.019999999552965164
+              Style: Squares
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/util/downsample/pointcloud
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: false
+            - Alpha: 0.9990000128746033
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 10
+                Min Value: -10
+                Value: true
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz_default_plugins/PointCloud2
+              Color: 85; 255; 0
+              Color Transformer: FlatColor
+              Decay Time: 0
+              Enabled: false
+              Invert Rainbow: false
+              Max Color: 85; 255; 127
+              Max Intensity: 0
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: Aligned
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 10
+              Size (m): 0.019999999552965164
+              Style: Squares
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/points_aligned
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: false
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: MonteCarloInitialPose
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/monte_carlo_initial_pose_marker
+              Value: true
+          Enabled: true
+          Name: NDT
+        - Class: rviz_common/Group
+          Displays:
+            - Buffer Size: 1000
+              Class: rviz_plugins::PoseHistory
+              Enabled: true
+              Line:
+                Alpha: 0.9990000128746033
+                Color: 0; 255; 255
+                Value: true
+                Width: 0.10000000149011612
+              Name: PoseHistory
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_twist_fusion_filter/pose
+              Value: true
+          Enabled: true
+          Name: EKF
+      Enabled: true
+      Name: Localization
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: DynamicObjects
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/object_recognition/detection/objects/visualization
+              Value: true
+          Enabled: false
+          Name: Detection
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: DynamicObjects
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/object_recognition/tracking/objects/visualization
+              Value: true
+          Enabled: false
+          Name: Tracking
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: DynamicObjects
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/object_recognition/objects/visualization
+              Value: true
+          Enabled: true
+          Name: Prediction
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/Image
+              Enabled: false
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: RecognitionResultOnImage
+              Normalize Range: true
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/traffic_light_recognition/debug/rois
+              Value: false
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: MapBasedDetectionResult
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers
+              Value: true
+          Enabled: true
+          Name: TrafficLight
+      Enabled: true
+      Name: Perception
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: RouteArea
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Transient Local
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/mission_planning/route_marker
+              Value: true
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.30000001192092896
+              Class: rviz_default_plugins/Pose
+              Color: 255; 25; 0
+              Enabled: true
+              Head Length: 0.30000001192092896
+              Head Radius: 0.5
+              Name: GoalPose
+              Shaft Length: 3
+              Shaft Radius: 0.20000000298023224
+              Shape: Axes
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/mission_planning/echo_back_goal_pose
+              Value: true
+          Enabled: true
+          Name: MissionPlanning
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_plugins/Trajectory
+              Color Border Vel Max: 3
+              Enabled: true
+              Name: ScenarioTrajectory
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/scenario_planning/trajectory
+              Value: true
+              View Path:
+                Alpha: 0.9990000128746033
+                Color: 0; 0; 0
+                Constant Color: false
+                Value: true
+                Width: 2
+              View Text Velocity:
+                Scale: 0.30000001192092896
+                Value: false
+              View Velocity:
+                Alpha: 0.9990000128746033
+                Color: 0; 0; 0
+                Constant Color: false
+                Scale: 0.30000001192092896
+                Value: true
+            - Class: rviz_common/Group
+              Displays:
+                - Class: rviz_common/Group
+                  Displays:
+                    - Class: rviz_plugins/Path
+                      Color Border Vel Max: 3
+                      Enabled: true
+                      Name: Path
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        Filter size: 10
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/path
+                      Value: true
+                      View Path:
+                        Alpha: 0.4000000059604645
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Value: true
+                        Width: 2
+                      View Velocity:
+                        Alpha: 0.4000000059604645
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Scale: 0.30000001192092896
+                        Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: false
+                      Name: Arrow
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path
+                      Value: false
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: Crosswalk
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: Intersection
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: Blind Spot
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: TrafficLight
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: VirtualTrafficLight
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: StopLine
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: DetectionArea
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: LaneChange
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/debug/predicted_path_markers
+                      Value: true
+                    - Class: rviz_plugins/Path
+                      Color Border Vel Max: 3
+                      Enabled: true
+                      Name: LaneChangeCandidate
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        Filter size: 10
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/lane_change_candidate_path
+                      Value: true
+                      View Path:
+                        Alpha: 0.30000001192092896
+                        Color: 115; 210; 22
+                        Constant Color: true
+                        Value: true
+                        Width: 2
+                      View Velocity:
+                        Alpha: 0.30000001192092896
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Scale: 0.30000001192092896
+                        Value: false
+                  Enabled: true
+                  Name: BehaviorPlanning
+                - Class: rviz_common/Group
+                  Displays:
+                    - Class: rviz_plugins/Trajectory
+                      Color Border Vel Max: 3
+                      Enabled: false
+                      Name: Trajectory
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        Filter size: 10
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/trajectory
+                      Value: false
+                      View Path:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Value: true
+                        Width: 2
+                      View Text Velocity:
+                        Scale: 0.30000001192092896
+                        Value: false
+                      View Velocity:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Scale: 0.30000001192092896
+                        Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: ObstaclePointCLoudStop
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: SurroundObstacleCheck
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: ObstacleAvoidance
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker
+                      Value: true
+                  Enabled: true
+                  Name: MotionPlanning
+              Enabled: true
+              Name: LaneDriving
+            - Class: rviz_common/Group
+              Displays:
+                - Alpha: 0.699999988079071
+                  Class: rviz_default_plugins/Map
+                  Color Scheme: map
+                  Draw Behind: false
+                  Enabled: false
+                  Name: Costmap
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid
+                  Update Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates
+                  Use Timestamp: false
+                  Value: false
+                - Alpha: 0.9990000128746033
+                  Arrow Length: 0.30000001192092896
+                  Axes Length: 0.30000001192092896
+                  Axes Radius: 0.009999999776482582
+                  Class: rviz_default_plugins/PoseArray
+                  Color: 255; 25; 0
+                  Enabled: true
+                  Head Length: 0.10000000149011612
+                  Head Radius: 0.20000000298023224
+                  Name: PartialPoseArray
+                  Shaft Length: 0.20000000298023224
+                  Shaft Radius: 0.05000000074505806
+                  Shape: Arrow (3D)
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array
+                  Value: true
+                - Alpha: 0.9990000128746033
+                  Arrow Length: 0.5
+                  Axes Length: 0.30000001192092896
+                  Axes Radius: 0.009999999776482582
+                  Class: rviz_default_plugins/PoseArray
+                  Color: 0; 0; 255
+                  Enabled: true
+                  Head Length: 0.10000000149011612
+                  Head Radius: 0.20000000298023224
+                  Name: PoseArray
+                  Shaft Length: 0.20000000298023224
+                  Shaft Radius: 0.05000000074505806
+                  Shape: Arrow (Flat)
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array
+                  Value: true
+              Enabled: true
+              Name: Parking
+          Enabled: true
+          Name: ScenarioPlanning
+      Enabled: false
+      Name: Planning
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_plugins/Trajectory
+          Color Border Vel Max: 3
+          Enabled: true
+          Name: Predicted Trajectory
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /control/trajectory_follower/predicted_trajectory
+          Value: true
+          View Path:
+            Alpha: 1
+            Color: 255; 255; 255
+            Constant Color: true
+            Value: true
+            Width: 0.05000000074505806
+          View Text Velocity:
+            Scale: 0.30000001192092896
+            Value: false
+          View Velocity:
+            Alpha: 1
+            Color: 0; 0; 0
+            Constant Color: false
+            Scale: 0.30000001192092896
+            Value: false
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Debug/MPC
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /control/trajectory_follower/mpc_follower/debug/markers
+          Value: false
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Debug/PurePursuit
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /control/trajectory_follower/pure_pursuit/debug/markers
+          Value: false
+      Enabled: false
+      Name: Control
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 0.30000001192092896
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 211; 215; 207
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: center_map
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/debug/map_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 0.30000001192092896
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 238; 238; 236
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: center map without wall pointcloud
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/debug/map_without_wall_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: center_accumulated
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensing/lidar/front_center/livox/accumulated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 0.30000001192092896
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 114; 159; 207
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: center_sensor
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/debug/sensor_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 115; 210; 22
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: center_sensor_without_wall
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.029999999329447746
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/debug/sensor_pointcloud_without_wall
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 239; 41; 41
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: center_calibrated
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.029999999329447746
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/debug/calibrated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: Center
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 51.6066780090332
+            Min Value: 36.993988037109375
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: left_map
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/debug/map_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: left_accumulated
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensing/lidar/front_left/livox/accumulated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 0.30000001192092896
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 114; 159; 207
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: left_sensor
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/debug/sensor_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 138; 226; 52
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: left_sensor_without_wall
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.029999999329447746
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/debug/sensor_pointcloud_without_wall
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 239; 41; 41
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: left_calibrated
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.029999999329447746
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/debug/calibrated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: Left
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: right_map
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/debug/map_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: right_accumulated
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensing/lidar/front_right/livox/accumulated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 0.30000001192092896
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 114; 159; 207
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: right_sensor
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/debug/sensor_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 138; 226; 52
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: right_sensor_without_wall
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.029999999329447746
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/debug/sensor_pointcloud_without_wall
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 239; 41; 41
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: right_calibrated
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.029999999329447746
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/debug/calibrated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: Right
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: velodyne_map
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/velodyne_top_base_link/debug/map_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 138; 226; 52
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: velodyne_sensor
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/velodyne_top_base_link/debug/sensor_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 239; 41; 41
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: velodyne_calibrated
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/velodyne_top_base_link/debug/calibrated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+      Enabled: false
+      Name: Top Velodyne
+  Enabled: true
+  Global Options:
+    Background Color: 10; 10; 10
+    Fixed Frame: map
+    Frame Rate: 5
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /planning/mission_planning/goal
+    - Class: rviz_plugins/PedestrianInitialPoseTool
+      Interactive: false
+      Pose Topic: /simulation/dummy_perception_publisher/object_info
+      Target Frame: <Fixed Frame>
+      Theta std deviation: 0.0872664600610733
+      Velocity: 0
+      X std deviation: 0.029999999329447746
+      Y std deviation: 0.029999999329447746
+      Z position: 1
+      Z std deviation: 0.029999999329447746
+    - Class: rviz_plugins/CarInitialPoseTool
+      Interactive: false
+      Pose Topic: /simulation/dummy_perception_publisher/object_info
+      Target Frame: <Fixed Frame>
+      Theta std deviation: 0.0872664600610733
+      Velocity: 3
+      X std deviation: 0.029999999329447746
+      Y std deviation: 0.029999999329447746
+      Z position: 0
+      Z std deviation: 0.029999999329447746
+    - Class: rviz_plugins/MissionCheckpointTool
+      Pose Topic: /planning/mission_planning/checkpoint
+      Theta std deviation: 0.2617993950843811
+      X std deviation: 0.5
+      Y std deviation: 0.5
+      Z position: 0
+    - Class: rviz_plugins/DeleteAllObjectsTool
+      Pose Topic: /simulation/dummy_perception_publisher/object_info
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Angle: 0.38499966263771057
+      Class: rviz_default_plugins/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 30.41171646118164
+      Target Frame: base_link
+      Value: TopDownOrtho (rviz_default_plugins)
+      X: -1.4694955348968506
+      Y: 6.260252475738525
+    Saved:
+      - Class: rviz_default_plugins/ThirdPersonFollower
+        Distance: 18
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Focal Point:
+          X: 0
+          Y: 0
+          Z: 0
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: ThirdPersonFollower
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 0.20000000298023224
+        Target Frame: base_link
+        Value: ThirdPersonFollower (rviz)
+        Yaw: 3.141592025756836
+      - Angle: 0
+        Class: rviz_default_plugins/TopDownOrtho
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Invert Z Axis: false
+        Name: TopDownOrtho
+        Near Clip Distance: 0.009999999776482582
+        Scale: 10
+        Target Frame: viewer
+        Value: TopDownOrtho (rviz)
+        X: 0
+        Y: 0
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1043
+  Hide Left Dock: false
+  Hide Right Dock: true
+  InitialPoseButtonPanel:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016200000359fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000359000000c900fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001b9000000c90000005c00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d0061006700650100000505000002680000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d0061006700650000000236000001450000002800ffffff000000010000015f00000359fc0200000003fb0000000a00560069006500770073000000003d00000359000000a400fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7a0000005afc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005afc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006180000035900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RecognitionResultOnImage:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1920
+  X: 0
+  Y: 0
diff --git a/sensor/extrinsic_map_based_calibrator/config/aip_x2/extrinsic_map_based_calib.yaml b/sensor/extrinsic_map_based_calibrator/config/aip_x2/extrinsic_map_based_calib.yaml
new file mode 100644
index 00000000..68dbe9da
--- /dev/null
+++ b/sensor/extrinsic_map_based_calibrator/config/aip_x2/extrinsic_map_based_calib.yaml
@@ -0,0 +1,31 @@
+/**:
+  ros__parameters:
+    preprocessing:
+      ransac:
+        max_iteration: 1000
+        voxel_grid_size: 0.3
+        distance_threshold: 0.01
+      clip_wall_pointcloud:
+        max_iteration: 100
+        max_correspondence_distance: 100.0
+        transformation_epsilon: 1.0e-10
+        euclidean_fitness_epsilon: 0.001
+        clipping_threshold: 0.05
+
+    grid_search:
+      x_range_max: 0.08
+      x_range_min: -0.08
+      x_range_resolution: 0.02
+      y_range_max: 0.08
+      y_range_min: -0.08
+      y_range_resolution: 0.02
+      yaw_range_max: 1.0
+      yaw_range_min: -1.0
+      yaw_range_resolution: 0.25
+      maximum_iteration: 1000
+      max_correspondence_distance: 100.0
+      transformation_epsilon: 1.0e-10
+      euclidean_fitness_epsilon: 0.001
+
+    map_based_calibrator:
+      debug_pub: true
diff --git a/sensor/extrinsic_map_based_calibrator/config/aip_x2/map_based_calib.rviz b/sensor/extrinsic_map_based_calibrator/config/aip_x2/map_based_calib.rviz
new file mode 100644
index 00000000..c58e8816
--- /dev/null
+++ b/sensor/extrinsic_map_based_calibrator/config/aip_x2/map_based_calib.rviz
@@ -0,0 +1,1952 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Sensing1/LiDAR1
+        - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1
+        - /left_calibrated1/Topic1
+      Splitter Ratio: 0.5829145908355713
+    Tree Height: 769
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded: ~
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel
+    Name: InitialPoseButtonPanel
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: map without wall
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/TF
+          Enabled: true
+          Frame Timeout: 15
+          Frames:
+            All Enabled: true
+            base_link:
+              Value: true
+            livox_front_center:
+              Value: true
+            livox_front_center_base_link:
+              Value: true
+            livox_front_left:
+              Value: true
+            livox_front_left_base_link:
+              Value: true
+            livox_front_right:
+              Value: true
+            livox_front_right_base_link:
+              Value: true
+            map:
+              Value: true
+            sensor_kit_base_link:
+              Value: true
+            velodyne_top:
+              Value: true
+            velodyne_top_base_link:
+              Value: true
+            viewer:
+              Value: true
+          Marker Scale: 1
+          Name: TF
+          Show Arrows: true
+          Show Axes: true
+          Show Names: true
+          Tree:
+            map:
+              base_link:
+                sensor_kit_base_link:
+                  livox_front_center_base_link:
+                    livox_front_center:
+                      {}
+                  livox_front_left_base_link:
+                    livox_front_left:
+                      {}
+                  livox_front_right_base_link:
+                    livox_front_right:
+                      {}
+                  velodyne_top_base_link:
+                    velodyne_top:
+                      {}
+              viewer:
+                {}
+          Update Interval: 0
+          Value: true
+        - Alpha: 0.5
+          Cell Size: 1
+          Class: rviz_default_plugins/Grid
+          Color: 160; 160; 164
+          Enabled: false
+          Line Style:
+            Line Width: 0.029999999329447746
+            Value: Lines
+          Name: Grid
+          Normal Cell Count: 0
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Plane: XY
+          Plane Cell Count: 10
+          Reference Frame: <Fixed Frame>
+          Value: false
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_plugins/SteeringAngle
+              Enabled: false
+              Left: 128
+              Length: 256
+              Name: SteeringAngle
+              Scale: 17
+              Text Color: 25; 255; 240
+              Top: 128
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /vehicle/status/steering
+              Value: false
+              Value Scale: 0.14999249577522278
+              Value height offset: 0
+            - Class: rviz_plugins/ConsoleMeter
+              Enabled: false
+              Left: 512
+              Length: 256
+              Name: ConsoleMeter
+              Text Color: 25; 255; 240
+              Top: 128
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /vehicle/status/twist
+              Value: false
+              Value Scale: 0.14999249577522278
+              Value height offset: 0
+            - Alpha: 0.9990000128746033
+              Class: rviz_plugins/VelocityHistory
+              Color Border Vel Max: 3
+              Constant Color:
+                Color: 255; 255; 255
+                Value: true
+              Enabled: true
+              Name: VelocityHistory
+              Scale: 0.30000001192092896
+              Timeout: 10
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /vehicle/status/twist
+              Value: true
+            - Alpha: 0.30000001192092896
+              Class: rviz_default_plugins/RobotModel
+              Collision Enabled: false
+              Description File: ""
+              Description Source: Topic
+              Description Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /robot_description
+              Enabled: true
+              Links:
+                All Links Enabled: true
+                Expand Joint Details: false
+                Expand Link Details: false
+                Expand Tree: false
+                Link Tree Style: Links in Alphabetic Order
+                base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                livox_front_center:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                livox_front_center_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                livox_front_left:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                livox_front_left_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                livox_front_right:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                livox_front_right_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                sensor_kit_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                velodyne_top:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                velodyne_top_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+              Name: VehicleModel
+              TF Prefix: ""
+              Update Interval: 0
+              Value: true
+              Visual Enabled: true
+            - Class: rviz_plugins/MaxVelocity
+              Enabled: true
+              Left: 595
+              Length: 96
+              Name: MaxVelocity
+              Text Color: 255; 255; 255
+              Top: 280
+              Topic: /planning/scenario_planning/current_max_velocity
+              Value: true
+              Value Scale: 0.25
+            - Class: rviz_plugins/TurnSignal
+              Enabled: false
+              Height: 256
+              Left: 196
+              Name: TurnSignal
+              Top: 350
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /control/turn_signal_cmd
+              Value: false
+              Width: 512
+          Enabled: true
+          Name: Vehicle
+      Enabled: true
+      Name: System
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 0.25
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 28.71826171875
+            Min Value: -7.4224700927734375
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 237
+          Min Color: 211; 215; 207
+          Min Intensity: 0
+          Name: PointCloudMap
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.019999999552965164
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /map/pointcloud_map
+          Use Fixed Frame: true
+          Use rainbow: false
+          Value: false
+        - Alpha: 0.25
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 211; 215; 207
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: map without wall
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /map/pointcloud_map_without_wall
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Lanelet2VectorMap
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /map/vector_map_marker
+          Value: false
+      Enabled: true
+      Name: Map
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Alpha: 0.4000000059604645
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 5
+                Min Value: -1
+                Value: false
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz_default_plugins/PointCloud2
+              Color: 255; 255; 255
+              Color Transformer: AxisColor
+              Decay Time: 0
+              Enabled: true
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: ConcatenatePointCloud
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 1
+              Size (m): 0.019999999552965164
+              Style: Points
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Best Effort
+                Value: /sensing/lidar/concatenated/pointcloud
+              Use Fixed Frame: false
+              Use rainbow: true
+              Value: true
+            - Alpha: 0.9990000128746033
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 15
+                Min Value: -2
+                Value: false
+              Axis: Z
+              Channel Name: z
+              Class: rviz_default_plugins/PointCloud2
+              Color: 200; 200; 200
+              Color Transformer: FlatColor
+              Decay Time: 0
+              Enabled: true
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 15
+              Min Color: 0; 0; 0
+              Min Intensity: -5
+              Name: NoGroundPointCloud
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 3
+              Size (m): 0.019999999552965164
+              Style: Points
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Best Effort
+                Value: /perception/obstacle_segmentation/pointcloud
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: true
+            - Alpha: 0.9990000128746033
+              Class: rviz_default_plugins/Polygon
+              Color: 25; 255; 0
+              Enabled: false
+              Name: MeasurementRange
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /sensing/lidar/crop_box_filter/crop_box_polygon
+              Value: false
+          Enabled: true
+          Name: LiDAR
+        - Class: rviz_common/Group
+          Displays:
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.10000000149011612
+              Class: rviz_default_plugins/PoseWithCovariance
+              Color: 233; 185; 110
+              Covariance:
+                Orientation:
+                  Alpha: 0.5
+                  Color: 255; 255; 127
+                  Color Style: Unique
+                  Frame: Local
+                  Offset: 1
+                  Scale: 1
+                  Value: false
+                Position:
+                  Alpha: 0.20000000298023224
+                  Color: 204; 51; 204
+                  Scale: 1
+                  Value: true
+                Value: true
+              Enabled: true
+              Head Length: 0.699999988079071
+              Head Radius: 1.2000000476837158
+              Name: PoseWithCovariance
+              Shaft Length: 1
+              Shaft Radius: 0.5
+              Shape: Arrow
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /sensing/gnss/pose_with_covariance
+              Value: true
+          Enabled: false
+          Name: GNSS
+      Enabled: false
+      Name: Sensing
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.10000000149011612
+              Class: rviz_default_plugins/PoseWithCovariance
+              Color: 0; 170; 255
+              Covariance:
+                Orientation:
+                  Alpha: 0.5
+                  Color: 255; 255; 127
+                  Color Style: Unique
+                  Frame: Local
+                  Offset: 1
+                  Scale: 1
+                  Value: true
+                Position:
+                  Alpha: 0.30000001192092896
+                  Color: 204; 51; 204
+                  Scale: 1
+                  Value: true
+                Value: true
+              Enabled: false
+              Head Length: 0.4000000059604645
+              Head Radius: 0.6000000238418579
+              Name: PoseWithCovInitial
+              Shaft Length: 0.6000000238418579
+              Shaft Radius: 0.30000001192092896
+              Shape: Arrow
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/initial_pose_with_covariance
+              Value: false
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.10000000149011612
+              Class: rviz_default_plugins/PoseWithCovariance
+              Color: 0; 255; 0
+              Covariance:
+                Orientation:
+                  Alpha: 0.5
+                  Color: 255; 255; 127
+                  Color Style: Unique
+                  Frame: Local
+                  Offset: 1
+                  Scale: 1
+                  Value: true
+                Position:
+                  Alpha: 0.30000001192092896
+                  Color: 204; 51; 204
+                  Scale: 1
+                  Value: true
+                Value: true
+              Enabled: false
+              Head Length: 0.4000000059604645
+              Head Radius: 0.6000000238418579
+              Name: PoseWithCovAligned
+              Shaft Length: 0.6000000238418579
+              Shaft Radius: 0.30000001192092896
+              Shape: Arrow
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/pose_with_covariance
+              Value: false
+            - Buffer Size: 200
+              Class: rviz_plugins::PoseHistory
+              Enabled: false
+              Line:
+                Alpha: 0.9990000128746033
+                Color: 170; 255; 127
+                Value: true
+                Width: 0.10000000149011612
+              Name: PoseHistory
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/pose
+              Value: false
+            - Alpha: 0.9990000128746033
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 10
+                Min Value: -10
+                Value: true
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz_default_plugins/PointCloud2
+              Color: 0; 255; 255
+              Color Transformer: ""
+              Decay Time: 0
+              Enabled: false
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: Initial
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 10
+              Size (m): 0.019999999552965164
+              Style: Squares
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/util/downsample/pointcloud
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: false
+            - Alpha: 0.9990000128746033
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 10
+                Min Value: -10
+                Value: true
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz_default_plugins/PointCloud2
+              Color: 85; 255; 0
+              Color Transformer: FlatColor
+              Decay Time: 0
+              Enabled: false
+              Invert Rainbow: false
+              Max Color: 85; 255; 127
+              Max Intensity: 0
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: Aligned
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 10
+              Size (m): 0.019999999552965164
+              Style: Squares
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/points_aligned
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: false
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: MonteCarloInitialPose
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/monte_carlo_initial_pose_marker
+              Value: true
+          Enabled: true
+          Name: NDT
+        - Class: rviz_common/Group
+          Displays:
+            - Buffer Size: 1000
+              Class: rviz_plugins::PoseHistory
+              Enabled: true
+              Line:
+                Alpha: 0.9990000128746033
+                Color: 0; 255; 255
+                Value: true
+                Width: 0.10000000149011612
+              Name: PoseHistory
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_twist_fusion_filter/pose
+              Value: true
+          Enabled: true
+          Name: EKF
+      Enabled: true
+      Name: Localization
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: DynamicObjects
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/object_recognition/detection/objects/visualization
+              Value: true
+          Enabled: false
+          Name: Detection
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: DynamicObjects
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/object_recognition/tracking/objects/visualization
+              Value: true
+          Enabled: false
+          Name: Tracking
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: DynamicObjects
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/object_recognition/objects/visualization
+              Value: true
+          Enabled: true
+          Name: Prediction
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/Image
+              Enabled: false
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: RecognitionResultOnImage
+              Normalize Range: true
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/traffic_light_recognition/debug/rois
+              Value: false
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: MapBasedDetectionResult
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers
+              Value: true
+          Enabled: true
+          Name: TrafficLight
+      Enabled: true
+      Name: Perception
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: RouteArea
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Transient Local
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/mission_planning/route_marker
+              Value: true
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.30000001192092896
+              Class: rviz_default_plugins/Pose
+              Color: 255; 25; 0
+              Enabled: true
+              Head Length: 0.30000001192092896
+              Head Radius: 0.5
+              Name: GoalPose
+              Shaft Length: 3
+              Shaft Radius: 0.20000000298023224
+              Shape: Axes
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/mission_planning/echo_back_goal_pose
+              Value: true
+          Enabled: true
+          Name: MissionPlanning
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_plugins/Trajectory
+              Color Border Vel Max: 3
+              Enabled: true
+              Name: ScenarioTrajectory
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/scenario_planning/trajectory
+              Value: true
+              View Path:
+                Alpha: 0.9990000128746033
+                Color: 0; 0; 0
+                Constant Color: false
+                Value: true
+                Width: 2
+              View Text Velocity:
+                Scale: 0.30000001192092896
+                Value: false
+              View Velocity:
+                Alpha: 0.9990000128746033
+                Color: 0; 0; 0
+                Constant Color: false
+                Scale: 0.30000001192092896
+                Value: true
+            - Class: rviz_common/Group
+              Displays:
+                - Class: rviz_common/Group
+                  Displays:
+                    - Class: rviz_plugins/Path
+                      Color Border Vel Max: 3
+                      Enabled: true
+                      Name: Path
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        Filter size: 10
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/path
+                      Value: true
+                      View Path:
+                        Alpha: 0.4000000059604645
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Value: true
+                        Width: 2
+                      View Velocity:
+                        Alpha: 0.4000000059604645
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Scale: 0.30000001192092896
+                        Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: false
+                      Name: Arrow
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path
+                      Value: false
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: Crosswalk
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: Intersection
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: Blind Spot
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: TrafficLight
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: VirtualTrafficLight
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: StopLine
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: DetectionArea
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: LaneChange
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/debug/predicted_path_markers
+                      Value: true
+                    - Class: rviz_plugins/Path
+                      Color Border Vel Max: 3
+                      Enabled: true
+                      Name: LaneChangeCandidate
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        Filter size: 10
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/lane_change_candidate_path
+                      Value: true
+                      View Path:
+                        Alpha: 0.30000001192092896
+                        Color: 115; 210; 22
+                        Constant Color: true
+                        Value: true
+                        Width: 2
+                      View Velocity:
+                        Alpha: 0.30000001192092896
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Scale: 0.30000001192092896
+                        Value: false
+                  Enabled: true
+                  Name: BehaviorPlanning
+                - Class: rviz_common/Group
+                  Displays:
+                    - Class: rviz_plugins/Trajectory
+                      Color Border Vel Max: 3
+                      Enabled: false
+                      Name: Trajectory
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        Filter size: 10
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/trajectory
+                      Value: false
+                      View Path:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Value: true
+                        Width: 2
+                      View Text Velocity:
+                        Scale: 0.30000001192092896
+                        Value: false
+                      View Velocity:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Scale: 0.30000001192092896
+                        Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: ObstaclePointCLoudStop
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: SurroundObstacleCheck
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: ObstacleAvoidance
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker
+                      Value: true
+                  Enabled: true
+                  Name: MotionPlanning
+              Enabled: true
+              Name: LaneDriving
+            - Class: rviz_common/Group
+              Displays:
+                - Alpha: 0.699999988079071
+                  Class: rviz_default_plugins/Map
+                  Color Scheme: map
+                  Draw Behind: false
+                  Enabled: false
+                  Name: Costmap
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid
+                  Update Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates
+                  Use Timestamp: false
+                  Value: false
+                - Alpha: 0.9990000128746033
+                  Arrow Length: 0.30000001192092896
+                  Axes Length: 0.30000001192092896
+                  Axes Radius: 0.009999999776482582
+                  Class: rviz_default_plugins/PoseArray
+                  Color: 255; 25; 0
+                  Enabled: true
+                  Head Length: 0.10000000149011612
+                  Head Radius: 0.20000000298023224
+                  Name: PartialPoseArray
+                  Shaft Length: 0.20000000298023224
+                  Shaft Radius: 0.05000000074505806
+                  Shape: Arrow (3D)
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array
+                  Value: true
+                - Alpha: 0.9990000128746033
+                  Arrow Length: 0.5
+                  Axes Length: 0.30000001192092896
+                  Axes Radius: 0.009999999776482582
+                  Class: rviz_default_plugins/PoseArray
+                  Color: 0; 0; 255
+                  Enabled: true
+                  Head Length: 0.10000000149011612
+                  Head Radius: 0.20000000298023224
+                  Name: PoseArray
+                  Shaft Length: 0.20000000298023224
+                  Shaft Radius: 0.05000000074505806
+                  Shape: Arrow (Flat)
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array
+                  Value: true
+              Enabled: true
+              Name: Parking
+          Enabled: true
+          Name: ScenarioPlanning
+      Enabled: true
+      Name: Planning
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_plugins/Trajectory
+          Color Border Vel Max: 3
+          Enabled: true
+          Name: Predicted Trajectory
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /control/trajectory_follower/predicted_trajectory
+          Value: true
+          View Path:
+            Alpha: 1
+            Color: 255; 255; 255
+            Constant Color: true
+            Value: true
+            Width: 0.05000000074505806
+          View Text Velocity:
+            Scale: 0.30000001192092896
+            Value: false
+          View Velocity:
+            Alpha: 1
+            Color: 0; 0; 0
+            Constant Color: false
+            Scale: 0.30000001192092896
+            Value: false
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Debug/MPC
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /control/trajectory_follower/mpc_follower/debug/markers
+          Value: false
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Debug/PurePursuit
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /control/trajectory_follower/pure_pursuit/debug/markers
+          Value: false
+      Enabled: true
+      Name: Control
+    - Alpha: 0.30000001192092896
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 211; 215; 207
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: center_map
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/debug/map_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 0.30000001192092896
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 238; 238; 236
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: center map without wall pointcloud
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/debug/map_without_wall_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: center_accumulated
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensing/lidar/front_center/livox/accumulated_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 0.30000001192092896
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 114; 159; 207
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: center_sensor
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/debug/sensor_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.5
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 115; 210; 22
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: center_sensor_without_wall
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/debug/sensor_pointcloud_without_wall
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 239; 41; 41
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: center_calibrated
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/debug/calibrated_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 51.6066780090332
+        Min Value: 36.993988037109375
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: left_map
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/debug/map_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: left_accumulated
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensing/lidar/front_left/livox/accumulated_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 0.30000001192092896
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 114; 159; 207
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: left_sensor
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/debug/sensor_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.5
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 138; 226; 52
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: left_sensor_without_wall
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/debug/sensor_pointcloud_without_wall
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.5
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 239; 41; 41
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: left_calibrated
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/debug/calibrated_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: right_map
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/debug/map_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: right_accumulated
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensing/lidar/front_right/livox/accumulated_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 0.30000001192092896
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 114; 159; 207
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: right_sensor
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/debug/sensor_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.5
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 138; 226; 52
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: right_sensor_without_wall
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/debug/sensor_pointcloud_without_wall
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.5
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 239; 41; 41
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: right_calibrated
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/debug/calibrated_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: velodyne_map
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/velodyne_top_base_link/debug/map_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 138; 226; 52
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: velodyne_sensor
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/velodyne_top_base_link/debug/sensor_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 239; 41; 41
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: velodyne_calibrated
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /sensor_kit/sensor_kit_base_link/velodyne_top_base_link/debug/calibrated_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 10; 10; 10
+    Fixed Frame: map
+    Frame Rate: 5
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /planning/mission_planning/goal
+    - Class: rviz_plugins/PedestrianInitialPoseTool
+      Pose Topic: /simulation/dummy_perception_publisher/object_info
+      Theta std deviation: 0.0872664600610733
+      Velocity: 0
+      X std deviation: 0.029999999329447746
+      Y std deviation: 0.029999999329447746
+      Z position: 1
+      Z std deviation: 0.029999999329447746
+    - Class: rviz_plugins/CarInitialPoseTool
+      Pose Topic: /simulation/dummy_perception_publisher/object_info
+      Theta std deviation: 0.0872664600610733
+      Velocity: 3
+      X std deviation: 0.029999999329447746
+      Y std deviation: 0.029999999329447746
+      Z position: 0
+      Z std deviation: 0.029999999329447746
+    - Class: rviz_plugins/MissionCheckpointTool
+      Pose Topic: /planning/mission_planning/checkpoint
+      Theta std deviation: 0.2617993950843811
+      X std deviation: 0.5
+      Y std deviation: 0.5
+      Z position: 0
+    - Class: rviz_plugins/DeleteAllObjectsTool
+      Pose Topic: /simulation/dummy_perception_publisher/object_info
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Angle: 0.35499992966651917
+      Class: rviz_default_plugins/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 44.257442474365234
+      Target Frame: base_link
+      Value: TopDownOrtho (rviz_default_plugins)
+      X: -3.1150197982788086
+      Y: 6.378146171569824
+    Saved:
+      - Class: rviz_default_plugins/ThirdPersonFollower
+        Distance: 18
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Focal Point:
+          X: 0
+          Y: 0
+          Z: 0
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: ThirdPersonFollower
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 0.20000000298023224
+        Target Frame: base_link
+        Value: ThirdPersonFollower (rviz)
+        Yaw: 3.141592025756836
+      - Angle: 0
+        Class: rviz_default_plugins/TopDownOrtho
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Invert Z Axis: false
+        Name: TopDownOrtho
+        Near Clip Distance: 0.009999999776482582
+        Scale: 10
+        Target Frame: viewer
+        Value: TopDownOrtho (rviz)
+        X: 0
+        Y: 0
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: true
+  InitialPoseButtonPanel:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001620000033efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000033e000000c900fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001b9000000c90000005c00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d0061006700650100000505000002680000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d0061006700650000000236000001450000002800ffffff000000010000015f0000033efc0200000003fb0000000a00560069006500770073000000003d0000033e000000a400fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7a0000005afc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005afc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005d00000033e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RecognitionResultOnImage:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1848
+  X: 1152
+  Y: 867
diff --git a/sensor/extrinsic_map_based_calibrator/config/aip_xx1/extrinsic_map_based_calib.yaml b/sensor/extrinsic_map_based_calibrator/config/aip_xx1/extrinsic_map_based_calib.yaml
new file mode 100644
index 00000000..6ef63ed2
--- /dev/null
+++ b/sensor/extrinsic_map_based_calibrator/config/aip_xx1/extrinsic_map_based_calib.yaml
@@ -0,0 +1,40 @@
+/**:
+  ros__parameters:
+    preprocessing:
+      ransac:
+        max_iteration: 1000
+        voxel_grid_size: 0.3
+        distance_threshold: 0.1
+      clip_wall_pointcloud:
+        max_iteration: 100
+        max_correspondence_distance: 100.0
+        transformation_epsilon: 1.0e-10
+        euclidean_fitness_epsilon: 0.001
+        clipping_threshold: 0.05
+
+    grid_search:
+      x_range_max: 0.05
+      x_range_min: -0.05
+      x_range_resolution: 0.025
+      y_range_max: 0.05
+      y_range_min: -0.05
+      y_range_resolution: 0.025
+      z_range_max: 0.05
+      z_range_min: -0.05
+      z_range_resolution: 0.05
+      roll_range_max: 0.5
+      roll_range_min: -0.5
+      roll_range_resolution: 0.25
+      pitch_range_max: 0.5
+      pitch_range_min: -0.5
+      pitch_range_resolution: 0.25
+      yaw_range_max: 0.5
+      yaw_range_min: -0.5
+      yaw_range_resolution: 0.25
+      maximum_iteration: 1000
+      max_correspondence_distance: 100.0
+      transformation_epsilon: 1.0e-10
+      euclidean_fitness_epsilon: 0.001
+
+    map_based_calibrator:
+      debug_pub: true
diff --git a/sensor/extrinsic_map_based_calibrator/config/aip_xx1/map_based_calib.rviz b/sensor/extrinsic_map_based_calibrator/config/aip_xx1/map_based_calib.rviz
new file mode 100644
index 00000000..a5470d39
--- /dev/null
+++ b/sensor/extrinsic_map_based_calibrator/config/aip_xx1/map_based_calib.rviz
@@ -0,0 +1,2058 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map1
+        - /Sensing1/LiDAR1
+        - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1
+        - /Velodyne rear1
+        - /Velodyne left1
+        - /Velodyne left1/accumulated_pointcloud1/Topic1
+        - /Velodyne right1
+      Splitter Ratio: 0.2235294133424759
+    Tree Height: 769
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded: ~
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel
+    Name: InitialPoseButtonPanel
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloudMap
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/TF
+          Enabled: true
+          Frame Timeout: 15
+          Frames:
+            All Enabled: true
+            base_link:
+              Value: true
+            camera0/camera_link:
+              Value: true
+            camera0/camera_optical_link:
+              Value: true
+            camera1/camera_link:
+              Value: true
+            camera1/camera_optical_link:
+              Value: true
+            camera2/camera_link:
+              Value: true
+            camera2/camera_optical_link:
+              Value: true
+            camera3/camera_link:
+              Value: true
+            camera3/camera_optical_link:
+              Value: true
+            camera4/camera_link:
+              Value: true
+            camera4/camera_optical_link:
+              Value: true
+            camera5/camera_link:
+              Value: true
+            camera5/camera_optical_link:
+              Value: true
+            gnss_base_link:
+              Value: true
+            gnss_link:
+              Value: true
+            livox_front_left:
+              Value: true
+            livox_front_left_base_link:
+              Value: true
+            livox_front_right:
+              Value: true
+            livox_front_right_base_link:
+              Value: true
+            map:
+              Value: true
+            ndt_base_link:
+              Value: true
+            sensor_kit_base_link:
+              Value: true
+            tamagawa/imu_link:
+              Value: true
+            traffic_light_left_camera/camera_link:
+              Value: true
+            traffic_light_left_camera/camera_optical_link:
+              Value: true
+            traffic_light_right_camera/camera_link:
+              Value: true
+            traffic_light_right_camera/camera_optical_link:
+              Value: true
+            velodyne_left:
+              Value: true
+            velodyne_left_base_link:
+              Value: true
+            velodyne_rear:
+              Value: true
+            velodyne_rear_base_link:
+              Value: true
+            velodyne_right:
+              Value: true
+            velodyne_right_base_link:
+              Value: true
+            velodyne_top:
+              Value: true
+            velodyne_top_base_link:
+              Value: true
+            viewer:
+              Value: true
+          Marker Scale: 1
+          Name: TF
+          Show Arrows: true
+          Show Axes: true
+          Show Names: true
+          Tree:
+            map:
+              base_link:
+                livox_front_left_base_link:
+                  livox_front_left:
+                    {}
+                livox_front_right_base_link:
+                  livox_front_right:
+                    {}
+                sensor_kit_base_link:
+                  camera0/camera_link:
+                    camera0/camera_optical_link:
+                      {}
+                  camera1/camera_link:
+                    camera1/camera_optical_link:
+                      {}
+                  camera2/camera_link:
+                    camera2/camera_optical_link:
+                      {}
+                  camera3/camera_link:
+                    camera3/camera_optical_link:
+                      {}
+                  camera4/camera_link:
+                    camera4/camera_optical_link:
+                      {}
+                  camera5/camera_link:
+                    camera5/camera_optical_link:
+                      {}
+                  gnss_link:
+                    {}
+                  tamagawa/imu_link:
+                    {}
+                  traffic_light_left_camera/camera_link:
+                    traffic_light_left_camera/camera_optical_link:
+                      {}
+                  traffic_light_right_camera/camera_link:
+                    traffic_light_right_camera/camera_optical_link:
+                      {}
+                  velodyne_left_base_link:
+                    velodyne_left:
+                      {}
+                  velodyne_right_base_link:
+                    velodyne_right:
+                      {}
+                  velodyne_top_base_link:
+                    velodyne_top:
+                      {}
+                velodyne_rear_base_link:
+                  velodyne_rear:
+                    {}
+              gnss_base_link:
+                {}
+              ndt_base_link:
+                {}
+              viewer:
+                {}
+          Update Interval: 0
+          Value: true
+        - Alpha: 0.5
+          Cell Size: 0.10000000149011612
+          Class: rviz_default_plugins/Grid
+          Color: 160; 160; 164
+          Enabled: false
+          Line Style:
+            Line Width: 0.029999999329447746
+            Value: Lines
+          Name: Grid
+          Normal Cell Count: 0
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Plane: XY
+          Plane Cell Count: 1000
+          Reference Frame: base_link
+          Value: false
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_plugins/SteeringAngle
+              Enabled: false
+              Left: 128
+              Length: 256
+              Name: SteeringAngle
+              Scale: 17
+              Text Color: 25; 255; 240
+              Top: 128
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /vehicle/status/steering
+              Value: false
+              Value Scale: 0.14999249577522278
+              Value height offset: 0
+            - Class: rviz_plugins/ConsoleMeter
+              Enabled: false
+              Left: 512
+              Length: 256
+              Name: ConsoleMeter
+              Text Color: 25; 255; 240
+              Top: 128
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /vehicle/status/twist
+              Value: false
+              Value Scale: 0.14999249577522278
+              Value height offset: 0
+            - Alpha: 0.9990000128746033
+              Class: rviz_plugins/VelocityHistory
+              Color Border Vel Max: 3
+              Constant Color:
+                Color: 255; 255; 255
+                Value: true
+              Enabled: true
+              Name: VelocityHistory
+              Scale: 0.30000001192092896
+              Timeout: 10
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /vehicle/status/twist
+              Value: true
+            - Alpha: 0.30000001192092896
+              Class: rviz_default_plugins/RobotModel
+              Collision Enabled: false
+              Description File: ""
+              Description Source: Topic
+              Description Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /robot_description
+              Enabled: true
+              Links:
+                All Links Enabled: true
+                Expand Joint Details: false
+                Expand Link Details: false
+                Expand Tree: false
+                Link Tree Style: Links in Alphabetic Order
+                base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                camera0/camera_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                camera0/camera_optical_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                camera1/camera_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                camera1/camera_optical_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                camera2/camera_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                camera2/camera_optical_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                camera3/camera_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                camera3/camera_optical_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                camera4/camera_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                camera4/camera_optical_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                camera5/camera_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                camera5/camera_optical_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                gnss_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                livox_front_left:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                livox_front_left_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                livox_front_right:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                livox_front_right_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                sensor_kit_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                tamagawa/imu_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                traffic_light_left_camera/camera_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                traffic_light_left_camera/camera_optical_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                traffic_light_right_camera/camera_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                traffic_light_right_camera/camera_optical_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                velodyne_left:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                velodyne_left_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                velodyne_rear:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                velodyne_rear_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                velodyne_right:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                velodyne_right_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                velodyne_top:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+                velodyne_top_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
+                  Value: true
+              Name: VehicleModel
+              TF Prefix: ""
+              Update Interval: 0
+              Value: true
+              Visual Enabled: true
+            - Class: rviz_plugins/MaxVelocity
+              Enabled: true
+              Left: 595
+              Length: 96
+              Name: MaxVelocity
+              Text Color: 255; 255; 255
+              Top: 280
+              Topic: /planning/scenario_planning/current_max_velocity
+              Value: true
+              Value Scale: 0.25
+            - Class: rviz_plugins/TurnSignal
+              Enabled: false
+              Height: 256
+              Left: 196
+              Name: TurnSignal
+              Top: 350
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /control/turn_signal_cmd
+              Value: false
+              Width: 512
+          Enabled: true
+          Name: Vehicle
+      Enabled: true
+      Name: System
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 0.75
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 28.71826171875
+            Min Value: -7.4224700927734375
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 237
+          Min Color: 211; 215; 207
+          Min Intensity: 0
+          Name: PointCloudMap
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.029999999329447746
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /map/pointcloud_map
+          Use Fixed Frame: true
+          Use rainbow: false
+          Value: true
+        - Alpha: 0.25
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 211; 215; 207
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: map without wall
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /map/pointcloud_map_without_wall
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Lanelet2VectorMap
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /map/vector_map_marker
+          Value: false
+      Enabled: true
+      Name: Map
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Alpha: 0.4000000059604645
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 5
+                Min Value: -1
+                Value: false
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz_default_plugins/PointCloud2
+              Color: 255; 255; 255
+              Color Transformer: AxisColor
+              Decay Time: 0
+              Enabled: true
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: ConcatenatePointCloud
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 1
+              Size (m): 0.019999999552965164
+              Style: Points
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Best Effort
+                Value: /sensing/lidar/concatenated/pointcloud
+              Use Fixed Frame: false
+              Use rainbow: true
+              Value: true
+            - Alpha: 0.9990000128746033
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 15
+                Min Value: -2
+                Value: false
+              Axis: Z
+              Channel Name: z
+              Class: rviz_default_plugins/PointCloud2
+              Color: 200; 200; 200
+              Color Transformer: FlatColor
+              Decay Time: 0
+              Enabled: true
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 15
+              Min Color: 0; 0; 0
+              Min Intensity: -5
+              Name: NoGroundPointCloud
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 3
+              Size (m): 0.019999999552965164
+              Style: Points
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Best Effort
+                Value: /perception/obstacle_segmentation/pointcloud
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: true
+            - Alpha: 0.9990000128746033
+              Class: rviz_default_plugins/Polygon
+              Color: 25; 255; 0
+              Enabled: false
+              Name: MeasurementRange
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /sensing/lidar/crop_box_filter/crop_box_polygon
+              Value: false
+          Enabled: true
+          Name: LiDAR
+        - Class: rviz_common/Group
+          Displays:
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.10000000149011612
+              Class: rviz_default_plugins/PoseWithCovariance
+              Color: 233; 185; 110
+              Covariance:
+                Orientation:
+                  Alpha: 0.5
+                  Color: 255; 255; 127
+                  Color Style: Unique
+                  Frame: Local
+                  Offset: 1
+                  Scale: 1
+                  Value: false
+                Position:
+                  Alpha: 0.20000000298023224
+                  Color: 204; 51; 204
+                  Scale: 1
+                  Value: true
+                Value: true
+              Enabled: true
+              Head Length: 0.699999988079071
+              Head Radius: 1.2000000476837158
+              Name: PoseWithCovariance
+              Shaft Length: 1
+              Shaft Radius: 0.5
+              Shape: Arrow
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /sensing/gnss/pose_with_covariance
+              Value: true
+          Enabled: false
+          Name: GNSS
+      Enabled: false
+      Name: Sensing
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.10000000149011612
+              Class: rviz_default_plugins/PoseWithCovariance
+              Color: 0; 170; 255
+              Covariance:
+                Orientation:
+                  Alpha: 0.5
+                  Color: 255; 255; 127
+                  Color Style: Unique
+                  Frame: Local
+                  Offset: 1
+                  Scale: 1
+                  Value: true
+                Position:
+                  Alpha: 0.30000001192092896
+                  Color: 204; 51; 204
+                  Scale: 1
+                  Value: true
+                Value: true
+              Enabled: false
+              Head Length: 0.4000000059604645
+              Head Radius: 0.6000000238418579
+              Name: PoseWithCovInitial
+              Shaft Length: 0.6000000238418579
+              Shaft Radius: 0.30000001192092896
+              Shape: Arrow
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/initial_pose_with_covariance
+              Value: false
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.10000000149011612
+              Class: rviz_default_plugins/PoseWithCovariance
+              Color: 0; 255; 0
+              Covariance:
+                Orientation:
+                  Alpha: 0.5
+                  Color: 255; 255; 127
+                  Color Style: Unique
+                  Frame: Local
+                  Offset: 1
+                  Scale: 1
+                  Value: true
+                Position:
+                  Alpha: 0.30000001192092896
+                  Color: 204; 51; 204
+                  Scale: 1
+                  Value: true
+                Value: true
+              Enabled: false
+              Head Length: 0.4000000059604645
+              Head Radius: 0.6000000238418579
+              Name: PoseWithCovAligned
+              Shaft Length: 0.6000000238418579
+              Shaft Radius: 0.30000001192092896
+              Shape: Arrow
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/pose_with_covariance
+              Value: false
+            - Buffer Size: 200
+              Class: rviz_plugins::PoseHistory
+              Enabled: false
+              Line:
+                Alpha: 0.9990000128746033
+                Color: 170; 255; 127
+                Value: true
+                Width: 0.10000000149011612
+              Name: PoseHistory
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/pose
+              Value: false
+            - Alpha: 0.9990000128746033
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 10
+                Min Value: -10
+                Value: true
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz_default_plugins/PointCloud2
+              Color: 0; 255; 255
+              Color Transformer: ""
+              Decay Time: 0
+              Enabled: false
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: Initial
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 10
+              Size (m): 0.019999999552965164
+              Style: Squares
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/util/downsample/pointcloud
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: false
+            - Alpha: 0.9990000128746033
+              Autocompute Intensity Bounds: true
+              Autocompute Value Bounds:
+                Max Value: 10
+                Min Value: -10
+                Value: true
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz_default_plugins/PointCloud2
+              Color: 85; 255; 0
+              Color Transformer: FlatColor
+              Decay Time: 0
+              Enabled: false
+              Invert Rainbow: false
+              Max Color: 85; 255; 127
+              Max Intensity: 0
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: Aligned
+              Position Transformer: XYZ
+              Selectable: true
+              Size (Pixels): 10
+              Size (m): 0.019999999552965164
+              Style: Squares
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/points_aligned
+              Use Fixed Frame: true
+              Use rainbow: true
+              Value: false
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: MonteCarloInitialPose
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_estimator/monte_carlo_initial_pose_marker
+              Value: true
+          Enabled: true
+          Name: NDT
+        - Class: rviz_common/Group
+          Displays:
+            - Buffer Size: 1000
+              Class: rviz_plugins::PoseHistory
+              Enabled: true
+              Line:
+                Alpha: 0.9990000128746033
+                Color: 0; 255; 255
+                Value: true
+                Width: 0.10000000149011612
+              Name: PoseHistory
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /localization/pose_twist_fusion_filter/pose
+              Value: true
+          Enabled: true
+          Name: EKF
+      Enabled: true
+      Name: Localization
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: DynamicObjects
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/object_recognition/detection/objects/visualization
+              Value: true
+          Enabled: false
+          Name: Detection
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: DynamicObjects
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/object_recognition/tracking/objects/visualization
+              Value: true
+          Enabled: false
+          Name: Tracking
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: DynamicObjects
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/object_recognition/objects/visualization
+              Value: true
+          Enabled: true
+          Name: Prediction
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/Image
+              Enabled: false
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: RecognitionResultOnImage
+              Normalize Range: true
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/traffic_light_recognition/debug/rois
+              Value: false
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: MapBasedDetectionResult
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers
+              Value: true
+          Enabled: true
+          Name: TrafficLight
+      Enabled: false
+      Name: Perception
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: RouteArea
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Transient Local
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/mission_planning/route_marker
+              Value: true
+            - Alpha: 0.9990000128746033
+              Axes Length: 1
+              Axes Radius: 0.30000001192092896
+              Class: rviz_default_plugins/Pose
+              Color: 255; 25; 0
+              Enabled: true
+              Head Length: 0.30000001192092896
+              Head Radius: 0.5
+              Name: GoalPose
+              Shaft Length: 3
+              Shaft Radius: 0.20000000298023224
+              Shape: Axes
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/mission_planning/echo_back_goal_pose
+              Value: true
+          Enabled: true
+          Name: MissionPlanning
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_plugins/Trajectory
+              Color Border Vel Max: 3
+              Enabled: true
+              Name: ScenarioTrajectory
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/scenario_planning/trajectory
+              Value: true
+              View Path:
+                Alpha: 0.9990000128746033
+                Color: 0; 0; 0
+                Constant Color: false
+                Value: true
+                Width: 2
+              View Text Velocity:
+                Scale: 0.30000001192092896
+                Value: false
+              View Velocity:
+                Alpha: 0.9990000128746033
+                Color: 0; 0; 0
+                Constant Color: false
+                Scale: 0.30000001192092896
+                Value: true
+            - Class: rviz_common/Group
+              Displays:
+                - Class: rviz_common/Group
+                  Displays:
+                    - Class: rviz_plugins/Path
+                      Color Border Vel Max: 3
+                      Enabled: true
+                      Name: Path
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        Filter size: 10
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/path
+                      Value: true
+                      View Path:
+                        Alpha: 0.4000000059604645
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Value: true
+                        Width: 2
+                      View Velocity:
+                        Alpha: 0.4000000059604645
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Scale: 0.30000001192092896
+                        Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: false
+                      Name: Arrow
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path
+                      Value: false
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: Crosswalk
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: Intersection
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: Blind Spot
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: TrafficLight
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: VirtualTrafficLight
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: StopLine
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: DetectionArea
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: LaneChange
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/debug/predicted_path_markers
+                      Value: true
+                    - Class: rviz_plugins/Path
+                      Color Border Vel Max: 3
+                      Enabled: true
+                      Name: LaneChangeCandidate
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        Filter size: 10
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/lane_change_candidate_path
+                      Value: true
+                      View Path:
+                        Alpha: 0.30000001192092896
+                        Color: 115; 210; 22
+                        Constant Color: true
+                        Value: true
+                        Width: 2
+                      View Velocity:
+                        Alpha: 0.30000001192092896
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Scale: 0.30000001192092896
+                        Value: false
+                  Enabled: true
+                  Name: BehaviorPlanning
+                - Class: rviz_common/Group
+                  Displays:
+                    - Class: rviz_plugins/Trajectory
+                      Color Border Vel Max: 3
+                      Enabled: false
+                      Name: Trajectory
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        Filter size: 10
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/trajectory
+                      Value: false
+                      View Path:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Value: true
+                        Width: 2
+                      View Text Velocity:
+                        Scale: 0.30000001192092896
+                        Value: false
+                      View Velocity:
+                        Alpha: 0.9990000128746033
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Scale: 0.30000001192092896
+                        Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: ObstaclePointCLoudStop
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: SurroundObstacleCheck
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker
+                      Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: ObstacleAvoidance
+                      Namespaces:
+                        {}
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker
+                      Value: true
+                  Enabled: true
+                  Name: MotionPlanning
+              Enabled: true
+              Name: LaneDriving
+            - Class: rviz_common/Group
+              Displays:
+                - Alpha: 0.699999988079071
+                  Class: rviz_default_plugins/Map
+                  Color Scheme: map
+                  Draw Behind: false
+                  Enabled: false
+                  Name: Costmap
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid
+                  Update Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates
+                  Use Timestamp: false
+                  Value: false
+                - Alpha: 0.9990000128746033
+                  Arrow Length: 0.30000001192092896
+                  Axes Length: 0.30000001192092896
+                  Axes Radius: 0.009999999776482582
+                  Class: rviz_default_plugins/PoseArray
+                  Color: 255; 25; 0
+                  Enabled: true
+                  Head Length: 0.10000000149011612
+                  Head Radius: 0.20000000298023224
+                  Name: PartialPoseArray
+                  Shaft Length: 0.20000000298023224
+                  Shaft Radius: 0.05000000074505806
+                  Shape: Arrow (3D)
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array
+                  Value: true
+                - Alpha: 0.9990000128746033
+                  Arrow Length: 0.5
+                  Axes Length: 0.30000001192092896
+                  Axes Radius: 0.009999999776482582
+                  Class: rviz_default_plugins/PoseArray
+                  Color: 0; 0; 255
+                  Enabled: true
+                  Head Length: 0.10000000149011612
+                  Head Radius: 0.20000000298023224
+                  Name: PoseArray
+                  Shaft Length: 0.20000000298023224
+                  Shaft Radius: 0.05000000074505806
+                  Shape: Arrow (Flat)
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array
+                  Value: true
+              Enabled: true
+              Name: Parking
+          Enabled: true
+          Name: ScenarioPlanning
+      Enabled: false
+      Name: Planning
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_plugins/Trajectory
+          Color Border Vel Max: 3
+          Enabled: true
+          Name: Predicted Trajectory
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /control/trajectory_follower/predicted_trajectory
+          Value: true
+          View Path:
+            Alpha: 1
+            Color: 255; 255; 255
+            Constant Color: true
+            Value: true
+            Width: 0.05000000074505806
+          View Text Velocity:
+            Scale: 0.30000001192092896
+            Value: false
+          View Velocity:
+            Alpha: 1
+            Color: 0; 0; 0
+            Constant Color: false
+            Scale: 0.30000001192092896
+            Value: false
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Debug/MPC
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /control/trajectory_follower/mpc_follower/debug/markers
+          Value: false
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Debug/PurePursuit
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /control/trajectory_follower/pure_pursuit/debug/markers
+          Value: false
+      Enabled: false
+      Name: Control
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: map
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/velodyne_right_base_link/debug/map_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: accumulated_pointcloud
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Best Effort
+            Value: /sensing/lidar/rear/pointcloud_raw/accumulated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 138; 226; 52
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: sensor
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.019999999552965164
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensors/base_link/velodyne_rear_base_link/debug/sensor_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 114; 159; 207
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: sensor_without_wall
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensors/base_link/velodyne_rear_base_link/debug/sensor_pointcloud_without_wall
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 0.75
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 239; 41; 41
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: calibrated_sensor
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.029999999329447746
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensors/base_link/velodyne_rear_base_link/debug/calibrated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: Velodyne rear
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 51.6066780090332
+            Min Value: 36.993988037109375
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: map
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/debug/map_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: accumulated_pointcloud
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Best Effort
+            Value: /sensing/lidar/left/pointcloud_raw/accumulated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 138; 226; 52
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: sensor
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.019999999552965164
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/velodyne_left_base_link/debug/sensor_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 114; 159; 207
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: sensor_without_wall
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/velodyne_left_base_link/debug/sensor_pointcloud_without_wall
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 0.75
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 239; 41; 41
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: calibrated_sensor
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.029999999329447746
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/velodyne_left_base_link/debug/calibrated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: Velodyne left
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: map
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/velodyne_right_base_link/debug/map_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: accumulated_pointcloud
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Best Effort
+            Value: /sensing/lidar/right/pointcloud_raw/accumulated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 138; 226; 52
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: sensor
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.019999999552965164
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/velodyne_right_base_link/debug/sensor_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 114; 159; 207
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: sensor_without_wall
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/velodyne_right_base_link/debug/sensor_pointcloud_without_wall
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Alpha: 0.75
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 239; 41; 41
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: calibrated_sensor
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.029999999329447746
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /sensor_kit/sensor_kit_base_link/velodyne_right_base_link/debug/calibrated_pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: Velodyne right
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 216
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Top Lidar
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /sensing/lidar/top/pointcloud_raw
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 10; 10; 10
+    Fixed Frame: map
+    Frame Rate: 5
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /planning/mission_planning/goal
+    - Class: rviz_plugins/PedestrianInitialPoseTool
+      Interactive: false
+      Pose Topic: /simulation/dummy_perception_publisher/object_info
+      Target Frame: <Fixed Frame>
+      Theta std deviation: 0.0872664600610733
+      Velocity: 0
+      X std deviation: 0.029999999329447746
+      Y std deviation: 0.029999999329447746
+      Z position: 1
+      Z std deviation: 0.029999999329447746
+    - Class: rviz_plugins/CarInitialPoseTool
+      Interactive: false
+      Pose Topic: /simulation/dummy_perception_publisher/object_info
+      Target Frame: <Fixed Frame>
+      Theta std deviation: 0.0872664600610733
+      Velocity: 3
+      X std deviation: 0.029999999329447746
+      Y std deviation: 0.029999999329447746
+      Z position: 0
+      Z std deviation: 0.029999999329447746
+    - Class: rviz_plugins/MissionCheckpointTool
+      Pose Topic: /planning/mission_planning/checkpoint
+      Theta std deviation: 0.2617993950843811
+      X std deviation: 0.5
+      Y std deviation: 0.5
+      Z position: 0
+    - Class: rviz_plugins/DeleteAllObjectsTool
+      Pose Topic: /simulation/dummy_perception_publisher/object_info
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Angle: 0.5699998736381531
+      Class: rviz_default_plugins/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 24.218605041503906
+      Target Frame: base_link
+      Value: TopDownOrtho (rviz_default_plugins)
+      X: 0
+      Y: 0
+    Saved:
+      - Class: rviz_default_plugins/ThirdPersonFollower
+        Distance: 18
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Focal Point:
+          X: 0
+          Y: 0
+          Z: 0
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: ThirdPersonFollower
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 0.20000000298023224
+        Target Frame: base_link
+        Value: ThirdPersonFollower (rviz)
+        Yaw: 3.141592025756836
+      - Angle: 0
+        Class: rviz_default_plugins/TopDownOrtho
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Invert Z Axis: false
+        Name: TopDownOrtho
+        Near Clip Distance: 0.009999999776482582
+        Scale: 10
+        Target Frame: viewer
+        Value: TopDownOrtho (rviz)
+        X: 0
+        Y: 0
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 1016
+  Hide Left Dock: true
+  Hide Right Dock: false
+  InitialPoseButtonPanel:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000004fd0000033efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000033e000000c900fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001b9000000c90000005c00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d0061006700650100000505000002680000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d0061006700650000000236000001450000002800ffffff000000010000015f0000033efc0200000003fb0000000a00560069006500770073010000003d0000033e000000a400fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7a0000005afc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000005afc0100000002fb0000000800540069006d0065010000000000000736000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005d10000033e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RecognitionResultOnImage:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1846
+  X: 74
+  Y: 27
diff --git a/sensor/extrinsic_map_based_calibrator/config/extrinsic_map_based_calib.yaml b/sensor/extrinsic_map_based_calibrator/config/extrinsic_map_based_calib.yaml
index 68dbe9da..4bf338db 100644
--- a/sensor/extrinsic_map_based_calibrator/config/extrinsic_map_based_calib.yaml
+++ b/sensor/extrinsic_map_based_calibrator/config/extrinsic_map_based_calib.yaml
@@ -19,6 +19,15 @@
       y_range_max: 0.08
       y_range_min: -0.08
       y_range_resolution: 0.02
+      z_range_max: 0.05
+      z_range_min: -0.05
+      z_range_resolution: 0.025
+      roll_range_max: 0.4
+      roll_range_min: -0.4
+      roll_range_resolution: 0.2
+      pitch_range_max: 0.4
+      pitch_range_min: -0.4
+      pitch_range_resolution: 0.2
       yaw_range_max: 1.0
       yaw_range_min: -1.0
       yaw_range_resolution: 0.25
diff --git a/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_calibrator.hpp b/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_calibrator.hpp
index 25a2c6a7..fe01688b 100644
--- a/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_calibrator.hpp
+++ b/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_calibrator.hpp
@@ -70,6 +70,7 @@ class ExtrinsicMapBasedCalibrator : public rclcpp::Node
   sensor_msgs::msg::PointCloud2 calibrated_pointcloud_msg_;
 
   bool is_debug_pub_ = true;
+  bool is_calibration_area_map_ = true;
 
   matchingResult calibrated_sensor_result_;
 
@@ -83,10 +84,12 @@ class ExtrinsicMapBasedCalibrator : public rclcpp::Node
     PointCloudT::Ptr & pcl_map,
     PointCloudT::Ptr & pcl_map_without_wall,
     PointCloudT::Ptr & pcl_sensor, const tf2::Transform & tf_initial_pose);
-  bool convertFromROSMsg(
+  bool preprocessing(
     PointCloudT::Ptr & pcl_map,
-    PointCloudT::Ptr & pcl_map_without_wall,
-    PointCloudT::Ptr & pcl_sensor);
+    PointCloudT::Ptr & pcl_sensor, const tf2::Transform & tf_initial_pose);
+  bool convertFromROSMsg(
+    PointCloudT::Ptr & pcl_pointcloud,
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg);
   void targetMapWithWallCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
   void targetMapWithoutWallCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
   void sourcePointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
@@ -101,6 +104,7 @@ class ExtrinsicMapBasedCalibrator : public rclcpp::Node
   void requestReceivedCallback(
     const std::shared_ptr<tier4_calibration_msgs::srv::ExtrinsicCalibrator::Request> request,
     const std::shared_ptr<tier4_calibration_msgs::srv::ExtrinsicCalibrator::Response> response);
+  void printTransform(const tf2::Transform & tf);
 };
 
 }  // namespace extrinsic_map_base_calibrator
diff --git a/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_preprocessing.hpp b/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_preprocessing.hpp
index 3b43a15d..451ec04b 100644
--- a/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_preprocessing.hpp
+++ b/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/extrinsic_map_based_preprocessing.hpp
@@ -54,8 +54,9 @@ struct PreprocessingConfig{
 class ExtrinsicMapBasedPreprocessing
 {
 private:
-  PreprocessingConfig config_;
+  PreprocessingConfig config_{};
   PointCloudMatcher matcher_;
+  matchingResult prematched_result_;
 
 public:
   explicit ExtrinsicMapBasedPreprocessing();
@@ -63,17 +64,18 @@ class ExtrinsicMapBasedPreprocessing
     const PointCloudT::Ptr & map_pointcloud_with_wall_pcl,
     const PointCloudT::Ptr & map_pointcloud_without_wall_pcl,
     const PointCloudT::Ptr & sensor_pointcloud_pcl);
-  void convertFromROSMsg(
-    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & sensor_msg_pointcloud,
-    PointCloudT::Ptr & pcl_pointcloud);
+  PointCloudT::Ptr preprocessing(
+    const PointCloudT::Ptr & map_point_cloud_with_wall,
+    const PointCloudT::Ptr & sensor_pointcloud_pcl);
   void downsamplingOnFloor(
     const PointCloudT::Ptr & pcl_sensor,
-    PointCloudT::Ptr & pcl_filtered_sensor);
+    PointCloudT::Ptr & pcl_filtered_sensor) const;
   PointCloudT::Ptr removeWallPointcloud(
     const PointCloudT::Ptr & sensor_point_cloud,
     const PointCloudT::Ptr & map_point_cloud_with_wall,
     const PointCloudT::Ptr & map_point_cloud_without_wall);
   void setConfig(PreprocessingConfig & config){config_ = config;};
+  matchingResult getPrematchedResult(){return prematched_result_;};
 };
 
 }  // namespace extrinsic_map_base_calibrator
diff --git a/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/grid_search_matching.hpp b/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/grid_search_matching.hpp
index 8fc6475a..7ef225cf 100644
--- a/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/grid_search_matching.hpp
+++ b/sensor/extrinsic_map_based_calibrator/include/extrinsic_map_based_calibrator/grid_search_matching.hpp
@@ -38,6 +38,15 @@ struct GridSearchConfig{
   double y_range_min_;
   double y_range_max_;
   double y_resolution_;
+  double z_range_min_;
+  double z_range_max_;
+  double z_resolution_;
+  double roll_range_min_;
+  double roll_range_max_;
+  double roll_resolution_;
+  double pitch_range_min_;
+  double pitch_range_max_;
+  double pitch_resolution_;
   double yaw_range_min_;
   double yaw_range_max_;
   double yaw_resolution_;
@@ -70,7 +79,9 @@ class GridSearchMatching
 
   matchingResult gridSearch(const PointCloudT::Ptr & map_pointCloud,
     const PointCloudT::Ptr & sensor_pointCloud);
-  std::vector<double> generateSearchElement(const double & range_min,
+  static Eigen::Matrix4d getMatrix4d(const double & x, const double & y, const double & z,
+    const double & roll, const double & pitch, const double & yaw);
+  static std::vector<double> generateSearchElement(const double & range_min,
     const double & range_max,
     const double & resolution);
 };
diff --git a/sensor/extrinsic_map_based_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_map_based_calibrator/launch/calibrator.launch.xml
index 4496d889..3a0de90d 100644
--- a/sensor/extrinsic_map_based_calibrator/launch/calibrator.launch.xml
+++ b/sensor/extrinsic_map_based_calibrator/launch/calibrator.launch.xml
@@ -7,6 +7,8 @@
   <arg name="input_map_without_wall_topic_name" />
   <arg name="input_source_topic_name" />
   <arg name="config_file_name" />
+  <arg name="use_calibration_area_map"/>
+  <arg name="sensor_model"/>
 
 
   <group>
@@ -14,7 +16,8 @@
     <node pkg="extrinsic_map_based_calibrator" exec="extrinsic_map_based_calibrator" name="extrinsic_map_based_calibrator" output="screen">
       <param name="parent_frame" value="$(var parent_frame)" />
       <param name="child_frame" value="$(var child_frame)" />
-      <param from="$(find-pkg-share extrinsic_map_based_calibrator)/config/$(var config_file_name)" />
+      <param name="use_calibration_area_map" value="$(var use_calibration_area_map)" />
+      <param from="$(find-pkg-share extrinsic_map_based_calibrator)/config/$(var sensor_model)/$(var config_file_name)" />
       <remap from="~/input/pointcloud_map_with_wall" to="$(var input_map_with_wall_topic_name)" />
       <remap from="~/input/pointcloud_map_without_wall" to="$(var input_map_without_wall_topic_name)" />
       <remap from="~/input/source_pointcloud" to="$(var input_source_topic_name)" />
diff --git a/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_calibrator.cpp b/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_calibrator.cpp
index 0a254a3b..668d171d 100644
--- a/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_calibrator.cpp
+++ b/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_calibrator.cpp
@@ -29,9 +29,10 @@ ExtrinsicMapBasedCalibrator::ExtrinsicMapBasedCalibrator(const rclcpp::NodeOptio
   parent_frame_ = this->declare_parameter("parent_frame", "");
   child_frame_ = this->declare_parameter("child_frame", "");
   is_debug_pub_ = this->declare_parameter<bool>("map_based_calibrator.debug_pub");
+  is_calibration_area_map_ = this->declare_parameter<bool>("use_calibration_area_map", "");
 
   {
-    PreprocessingConfig config;
+    PreprocessingConfig config{};
     config.ransac_config.max_iteration = this->declare_parameter<int>("preprocessing.ransac.max_iteration");
     config.ransac_config.voxel_grid_size = this->declare_parameter<double>("preprocessing.ransac.voxel_grid_size");
     config.ransac_config.distance_threshold = this->declare_parameter<double>("preprocessing.ransac.distance_threshold");
@@ -55,6 +56,15 @@ ExtrinsicMapBasedCalibrator::ExtrinsicMapBasedCalibrator(const rclcpp::NodeOptio
     config.y_range_max_ = this->declare_parameter<double>("grid_search.y_range_max");
     config.y_range_min_ = this->declare_parameter<double>("grid_search.y_range_min");
     config.y_resolution_ = this->declare_parameter<double>("grid_search.y_range_resolution");
+    config.z_range_max_ = this->declare_parameter<double>("grid_search.z_range_max");
+    config.z_range_min_ = this->declare_parameter<double>("grid_search.z_range_min");
+    config.z_resolution_ = this->declare_parameter<double>("grid_search.z_range_resolution");
+    config.roll_range_max_ = this->declare_parameter<double>("grid_search.roll_range_max");
+    config.roll_range_min_ = this->declare_parameter<double>("grid_search.roll_range_min");
+    config.roll_resolution_ = this->declare_parameter<double>("grid_search.roll_range_resolution");
+    config.pitch_range_max_ = this->declare_parameter<double>("grid_search.pitch_range_max");
+    config.pitch_range_min_ = this->declare_parameter<double>("grid_search.pitch_range_min");
+    config.pitch_resolution_ = this->declare_parameter<double>("grid_search.pitch_range_resolution");
     config.yaw_range_max_ = this->declare_parameter<double>("grid_search.yaw_range_max");
     config.yaw_range_min_ = this->declare_parameter<double>("grid_search.yaw_range_min");
     config.yaw_resolution_ = this->declare_parameter<double>("grid_search.yaw_range_resolution");
@@ -114,6 +124,8 @@ ExtrinsicMapBasedCalibrator::ExtrinsicMapBasedCalibrator(const rclcpp::NodeOptio
     calibrated_pointcloud_pub_ =
       this->create_publisher<sensor_msgs::msg::PointCloud2>("debug/calibrated_pointcloud", map_qos);
   }
+  // wait for other node
+  rclcpp::sleep_for(10s);
 }
 
 bool ExtrinsicMapBasedCalibrator::mapBasedCalibration(const tf2::Transform & tf_initial_pose)
@@ -121,20 +133,27 @@ bool ExtrinsicMapBasedCalibrator::mapBasedCalibration(const tf2::Transform & tf_
   if (!map_with_wall_pointcloud_msg_) {
     RCLCPP_ERROR(this->get_logger(), "Can not received point cloud map topic");
     return false;
-  } else if (!map_without_wall_pointcloud_msg_) {
+  }
+  if (!map_without_wall_pointcloud_msg_ && is_calibration_area_map_) {
     RCLCPP_ERROR(this->get_logger(), "Can not received point cloud map topic");
     return false;
-  } else if (!sensor_pointcloud_msg_) {
+  }
+  if (!sensor_pointcloud_msg_) {
     RCLCPP_ERROR(this->get_logger(), "Can not received pandar left upper point cloud topic");
     return false;
   }
+
   if (map_with_wall_pointcloud_msg_->height == 0) {
     RCLCPP_ERROR(this->get_logger(), "Can not received point cloud map topic");
     return false;
-  } else if (map_without_wall_pointcloud_msg_->height == 0) {
-    RCLCPP_ERROR(this->get_logger(), "Can not received point cloud map topic");
-    return false;
-  } else if (sensor_pointcloud_msg_->height == 0) {
+  }
+  if ( is_calibration_area_map_ ) {
+    if( map_without_wall_pointcloud_msg_->height == 0 ) {
+      RCLCPP_ERROR(this->get_logger(), "Can not received point cloud map topic");
+      return false;
+    }
+  }
+  if (sensor_pointcloud_msg_->height == 0) {
     RCLCPP_ERROR(this->get_logger(), "Can not received pandar left upper point cloud topic");
     return false;
   }
@@ -142,16 +161,22 @@ bool ExtrinsicMapBasedCalibrator::mapBasedCalibration(const tf2::Transform & tf_
   PointCloudT::Ptr pcl_map(new PointCloudT);
   PointCloudT::Ptr pcl_map_without_wall(new PointCloudT);
   PointCloudT::Ptr pcl_sensor(new PointCloudT);
-  if (!preprocessing(pcl_map, pcl_map_without_wall, pcl_sensor, tf_initial_pose)) {
-    return false;
+  if(is_calibration_area_map_) {
+    if (!preprocessing(pcl_map, pcl_map_without_wall, pcl_sensor, tf_initial_pose)) {
+      return false;
+    }
+    grid_search_matching_.executeGridSearchMatching(pcl_map_without_wall, pcl_sensor);
+  } else {
+    if (!preprocessing(pcl_map, pcl_sensor, tf_initial_pose)) {
+      return false;
+    }
+    grid_search_matching_.executeGridSearchMatching(pcl_map, pcl_sensor);
   }
 
-  grid_search_matching_.executeGridSearchMatching(pcl_map_without_wall, pcl_sensor);
-
-  matchingResult final_result = grid_search_matching_.getRematchedResult();
+  calibrated_sensor_result_ = grid_search_matching_.getRematchedResult();
 
   PointCloudT::Ptr calibrated_pointcloud(new PointCloudT);
-  pcl::transformPointCloud(*pcl_sensor, *calibrated_pointcloud, final_result.transformation_matrix);
+  pcl::transformPointCloud(*pcl_sensor, *calibrated_pointcloud, calibrated_sensor_result_.transformation_matrix);
 
   publishPointCloud(calibrated_pointcloud, calibrated_pointcloud_pub_);
   pcl::toROSMsg(*calibrated_pointcloud, calibrated_pointcloud_msg_);
@@ -167,7 +192,10 @@ bool ExtrinsicMapBasedCalibrator::preprocessing(
   PointCloudT::Ptr pcl_map_tmp(new PointCloudT);
   PointCloudT::Ptr pcl_map_without_wall_tmp(new PointCloudT);
   PointCloudT::Ptr pcl_sensor_tmp(new PointCloudT);
-  if (!convertFromROSMsg(pcl_map_tmp, pcl_map_without_wall_tmp, pcl_sensor_tmp)) {
+  if (!convertFromROSMsg(pcl_map_tmp, map_with_wall_pointcloud_msg_) ||
+    !convertFromROSMsg(pcl_map_without_wall_tmp, map_without_wall_pointcloud_msg_) ||
+    !convertFromROSMsg(pcl_sensor_tmp, sensor_pointcloud_msg_)) {
+    RCLCPP_ERROR(this->get_logger(), "Fault convert ros message to pcl");
     return false;
   }
   // transform each frame to parent_frame
@@ -189,34 +217,40 @@ bool ExtrinsicMapBasedCalibrator::preprocessing(
   return true;
 }
 
-bool ExtrinsicMapBasedCalibrator::convertFromROSMsg(
+bool ExtrinsicMapBasedCalibrator::preprocessing(
   PointCloudT::Ptr & pcl_map,
-  PointCloudT::Ptr & pcl_map_without_wall,
-  PointCloudT::Ptr & pcl_sensor)
+  PointCloudT::Ptr & pcl_sensor, const tf2::Transform & tf_initial_pose)
 {
-  {
-    std::lock_guard<std::mutex> message_lock(mutex_);
-    if (!map_with_wall_pointcloud_msg_) {
-      RCLCPP_ERROR(this->get_logger(), "cant get point cloud map");
-      return false;
-    }
-    pcl::fromROSMsg(*map_with_wall_pointcloud_msg_, *pcl_map);
-  }
-  {
-    std::lock_guard<std::mutex> message_lock(mutex_);
-    if (!map_without_wall_pointcloud_msg_) {
-      RCLCPP_ERROR(this->get_logger(), "cant get point cloud map without wall");
-      return false;
-    }
-    pcl::fromROSMsg(*map_without_wall_pointcloud_msg_, *pcl_map_without_wall);
+  PointCloudT::Ptr pcl_map_tmp(new PointCloudT);
+  PointCloudT::Ptr pcl_sensor_tmp(new PointCloudT);
+  if (!convertFromROSMsg(pcl_map_tmp, map_with_wall_pointcloud_msg_) ||
+    !convertFromROSMsg(pcl_sensor_tmp, sensor_pointcloud_msg_)) {
+    RCLCPP_ERROR(this->get_logger(), "Fault convert ros message to pcl");
+    return false;
   }
+  // transform each frame to parent_frame
+  PointCloudT::Ptr transformed_sensor(new PointCloudT);
+  pcl_ros::transformPointCloud(parent_frame_, *pcl_map_tmp, *pcl_map, tf_buffer_);
+  pcl_ros::transformPointCloud(*pcl_sensor_tmp, *transformed_sensor, tf_initial_pose);
+
+  publishPointCloud(pcl_map, map_pointcloud_pub_);
+  publishPointCloud(transformed_sensor, sensor_pointcloud_pub_);
+
+  pcl_sensor = preprocessing_.preprocessing(pcl_map, transformed_sensor);
+  publishPointCloud(pcl_sensor, sensor_pointcloud_without_wall_pub_);
+  return true;
+}
+
+bool ExtrinsicMapBasedCalibrator::convertFromROSMsg(
+  PointCloudT::Ptr & pcl_pointcloud,
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg)
+{
   {
     std::lock_guard<std::mutex> message_lock(mutex_);
-    if (!sensor_pointcloud_msg_) {
-      RCLCPP_ERROR(this->get_logger(), "cant get sensor cloud");
+    if (!msg) {
       return false;
     }
-    pcl::fromROSMsg(*sensor_pointcloud_msg_, *pcl_sensor);
+    pcl::fromROSMsg(*msg, *pcl_pointcloud);
   }
   return true;
 }
@@ -277,16 +311,14 @@ void ExtrinsicMapBasedCalibrator::requestReceivedCallback(
   // Wait for subscription topic
   while (rclcpp::ok() &&
     (!sensor_pointcloud_msg_ || !map_with_wall_pointcloud_msg_ ||
-    !map_without_wall_pointcloud_msg_ ))
+    (!map_without_wall_pointcloud_msg_ && is_calibration_area_map_)))
   {
     if (!map_with_wall_pointcloud_msg_) {
       RCLCPP_WARN_SKIPFIRST(this->get_logger(), "Can not received point cloud map topic");
-    } else if (!map_without_wall_pointcloud_msg_) {
+    } else if (!map_without_wall_pointcloud_msg_ && is_calibration_area_map_) {
       RCLCPP_WARN_SKIPFIRST(
         this->get_logger(), "Can not received point cloud map without wall topic");
     } else if (!sensor_pointcloud_msg_) {
-      RCLCPP_WARN_SKIPFIRST(
-        this->get_logger(), "map size %ld", map_with_wall_pointcloud_msg_->data.size());
       RCLCPP_WARN_SKIPFIRST(
         this->get_logger(), "Can not received sensor point cloud topic");
     }
@@ -298,30 +330,58 @@ void ExtrinsicMapBasedCalibrator::requestReceivedCallback(
   tf2::fromMsg(request->initial_pose, tf_initial_pose);
 
   // execute gicp matching
-  RCLCPP_DEBUG_STREAM(this->get_logger(), "--- Execute gicp Matching ---");
+  RCLCPP_DEBUG_STREAM(this->get_logger(), "--- Execute map based calibration ---");
   bool is_matching = mapBasedCalibration(tf_initial_pose);
-  if ( is_matching) {
-    response->success = true;
-  } else {
-    response->success = false;
-  }
+  response->success = is_matching;
 
   // set result to response
+  // printTransform(tf_initial_pose);
   response->debug_pointcloud.header.frame_id = parent_frame_;
   response->debug_pointcloud = calibrated_pointcloud_msg_;
-  response->result_pose.position.x = calibrated_sensor_result_.transformation_matrix(0, 3);
-  response->result_pose.position.y = calibrated_sensor_result_.transformation_matrix(1, 3);
-  response->result_pose.position.z = calibrated_sensor_result_.transformation_matrix(2, 3);
+  matchingResult prematch = preprocessing_.getPrematchedResult();
+  tf2::Vector3 trans_pre( prematch.transformation_matrix(0, 3),
+    prematch.transformation_matrix(1, 3),
+    prematch.transformation_matrix(2, 3));
+  Eigen::Matrix3d R_pre = prematch.transformation_matrix.block(0, 0, 3, 3);
+  Eigen::Quaterniond q_pre(R_pre);
+
+  tf2::Quaternion Qtf_pre(q_pre.x(), q_pre.y(), q_pre.z(), q_pre.w());
+  tf2::Transform tf_prematch(Qtf_pre, trans_pre);
+
+  tf2::Vector3 trans( calibrated_sensor_result_.transformation_matrix(0, 3),
+    calibrated_sensor_result_.transformation_matrix(1, 3),
+    calibrated_sensor_result_.transformation_matrix(2, 3));
   Eigen::Matrix3d R = calibrated_sensor_result_.transformation_matrix.block(0, 0, 3, 3);
   Eigen::Quaterniond q(R);
-  response->result_pose.orientation.x = q.x();
-  response->result_pose.orientation.y = q.y();
-  response->result_pose.orientation.z = q.z();
-  response->result_pose.orientation.w = q.w();
+
+  tf2::Quaternion Qtf(q.x(), q.y(), q.z(), q.w());
+  tf2::Transform result_tf(Qtf, trans);
+  // printTransform(result_tf);
+  result_tf = result_tf * tf_prematch * tf_initial_pose;
+  // printTransform(result_tf);
+  tf2::toMsg(result_tf, response->result_pose);
 
   response->score = calibrated_sensor_result_.score;
 }
 
+void ExtrinsicMapBasedCalibrator::printTransform(const tf2::Transform & tf)
+{
+  RCLCPP_INFO_STREAM(this->get_logger(), "trans");
+  RCLCPP_INFO_STREAM(this->get_logger(), tf.getOrigin().x()
+    << ", " << tf.getOrigin().y()
+    << ", " << tf.getOrigin().z());
+
+  tf2::Matrix3x3 tf2_matrix(tf.getRotation());
+  double roll;
+double pitch;
+double yaw;
+  tf2_matrix.getRPY(roll, pitch, yaw);
+  RCLCPP_INFO_STREAM(this->get_logger(), "rotate");
+  RCLCPP_INFO_STREAM(this->get_logger(), roll
+    << ", " << pitch
+    << ", " << yaw);
+}
+
 }  // namespace extrinsic_map_base_calibrator
 
 int main(int argc, char ** argv)
diff --git a/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_preprocessing.cpp b/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_preprocessing.cpp
index 2768089c..bd792745 100644
--- a/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_preprocessing.cpp
+++ b/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_preprocessing.cpp
@@ -35,9 +35,26 @@ PointCloudT::Ptr ExtrinsicMapBasedPreprocessing::preprocessing(
     map_pointcloud_without_wall_pcl);
 }
 
+PointCloudT::Ptr ExtrinsicMapBasedPreprocessing::preprocessing(
+  const PointCloudT::Ptr & map_pointcloud_with_wall,
+  const PointCloudT::Ptr & sensor_pointcloud_pcl)
+{
+  // matching of sensor cloud and map cloud
+  matcher_.setParameter(config_.clip_config.matching_config);
+  prematched_result_ = matcher_.ICPMatching(map_pointcloud_with_wall, sensor_pointcloud_pcl);
+  PointCloudT::Ptr matched_sensor_pointcloud(new PointCloudT);
+  pcl::transformPointCloud( *sensor_pointcloud_pcl, *matched_sensor_pointcloud, prematched_result_.transformation_matrix);
+
+  PointCloudT::Ptr filtered_sensor_pointcloud(new PointCloudT);
+  // downsampling on the floor
+  downsamplingOnFloor(matched_sensor_pointcloud, filtered_sensor_pointcloud);
+
+  return filtered_sensor_pointcloud;
+}
+
 void ExtrinsicMapBasedPreprocessing::downsamplingOnFloor(
   const PointCloudT::Ptr & pcl_sensor,
-  PointCloudT::Ptr & pcl_filtered_sensor)
+  PointCloudT::Ptr & pcl_filtered_sensor) const
 {
   // ransac
   pcl::PointIndices::Ptr output_inliers(new pcl::PointIndices);
@@ -83,8 +100,8 @@ PointCloudT::Ptr ExtrinsicMapBasedPreprocessing::removeWallPointcloud(
   // matching of sensor cloud and map cloud
   matcher_.setParameter(config_.clip_config.matching_config);
   PointCloudT matched_sensor_point_cloud;
-  matchingResult result = matcher_.GICPMatching(map_point_cloud_with_wall, sensor_point_cloud);
-  pcl::transformPointCloud( *sensor_point_cloud, matched_sensor_point_cloud, result.transformation_matrix);
+  prematched_result_ = matcher_.ICPMatching(map_point_cloud_with_wall, sensor_point_cloud);
+  pcl::transformPointCloud( *sensor_point_cloud, matched_sensor_point_cloud, prematched_result_.transformation_matrix);
 
   // return matched_sensor_point_cloud.makeShared();
   // clip overlapped cloud
diff --git a/sensor/extrinsic_map_based_calibrator/src/grid_search_matching.cpp b/sensor/extrinsic_map_based_calibrator/src/grid_search_matching.cpp
index e435dcc0..9b376af4 100644
--- a/sensor/extrinsic_map_based_calibrator/src/grid_search_matching.cpp
+++ b/sensor/extrinsic_map_based_calibrator/src/grid_search_matching.cpp
@@ -15,6 +15,7 @@
 #include "extrinsic_map_based_calibrator/grid_search_matching.hpp"
 
 #include "tier4_autoware_utils/math/unit_conversion.hpp"
+#include <omp.h>
 
 namespace extrinsic_map_base_calibrator
 {
@@ -57,43 +58,53 @@ bool GridSearchMatching::executeGridSearchMatching(
 matchingResult GridSearchMatching::gridSearch(
   const PointCloudT::Ptr & map_pointcloud, const PointCloudT::Ptr & sensor_pointcloud)
 {
-  std::vector<double> x_score =
+  std::vector<double> x_elements =
     generateSearchElement(config_.x_range_min_, config_.x_range_max_, config_.x_resolution_);
-  std::vector<double> y_score =
+  std::vector<double> y_elements =
     generateSearchElement(config_.y_range_min_, config_.y_range_max_, config_.y_resolution_);
-  std::vector<double> yaw_score =
+  std::vector<double> z_elements =
+    generateSearchElement(config_.z_range_min_, config_.z_range_max_, config_.z_resolution_);
+  std::vector<double> roll_elements =
+    generateSearchElement(config_.roll_range_min_, config_.roll_range_max_, config_.roll_resolution_);
+  std::vector<double> pitch_elements =
+    generateSearchElement(config_.pitch_range_min_, config_.pitch_range_max_, config_.pitch_resolution_);
+  std::vector<double> yaw_elements =
     generateSearchElement(config_.yaw_range_min_, config_.yaw_range_max_, config_.yaw_resolution_);
+
   pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_map(new pcl::search::KdTree<pcl::PointXYZ>);
   tree_map->setInputCloud(map_pointcloud);
   matchingResult min_score_result;
   min_score_result.score = DBL_MAX;
-
-  for (size_t i = 0; i < x_score.size(); ++i) {
-    for (size_t j = 0; j < y_score.size(); ++j) {
-      for (size_t k = 0; k < yaw_score.size(); ++k) {
-        Eigen::Matrix4d transformation_score_matrix = Eigen::Matrix4d::Identity();
-
-        double theta_score = tier4_autoware_utils::deg2rad(yaw_score.at(k));
-        transformation_score_matrix(0, 0) = std::cos(theta_score);
-        transformation_score_matrix(0, 1) = -sin(theta_score);
-        transformation_score_matrix(1, 0) = sin(theta_score);
-        transformation_score_matrix(1, 1) = std::cos(theta_score);
-        transformation_score_matrix(0, 3) = x_score.at(i);
-        transformation_score_matrix(1, 3) = y_score.at(j);
-        transformation_score_matrix(2, 3) = 0.0;
-
-        PointCloudT::Ptr translated_cloud(new PointCloudT);
-        pcl::transformPointCloud(
-          *sensor_pointcloud, *translated_cloud, transformation_score_matrix);
-
-        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_sensor(new pcl::search::KdTree<pcl::PointXYZ>);
-        tree_sensor->setInputCloud(translated_cloud);
-        double tmp_score =
-          matcher_.getFitnessScore(map_pointcloud, translated_cloud, tree_map, tree_sensor);
-
-        if (tmp_score < min_score_result.score) {
-          min_score_result.score = tmp_score;
-          min_score_result.transformation_matrix = transformation_score_matrix;
+  #pragma omp parallel for
+  for (const auto & x_element: x_elements) {
+    #pragma omp parallel for
+    for (const auto & y_element: y_elements) {
+      #pragma omp parallel for
+      for  (const auto & z_element: z_elements) {
+        #pragma omp parallel for
+        for  (const auto & roll_element: roll_elements) {
+          #pragma omp parallel for
+          for  (const auto & pitch_element: pitch_elements) {
+            #pragma omp parallel for
+            for  (const auto & yaw_element: yaw_elements) {
+              Eigen::Matrix4d transformation_score_matrix = getMatrix4d(x_element, y_element, z_element, roll_element, pitch_element, yaw_element);
+              PointCloudT::Ptr translated_cloud(new PointCloudT);
+              pcl::transformPointCloud(
+                *sensor_pointcloud, *translated_cloud, transformation_score_matrix);
+
+              pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_sensor(new pcl::search::KdTree<pcl::PointXYZ>);
+              tree_sensor->setInputCloud(translated_cloud);
+              double tmp_score =
+                matcher_.getFitnessScore(map_pointcloud, translated_cloud, tree_map, tree_sensor);
+              #pragma omp critical (score)
+              {
+                if (tmp_score < min_score_result.score) {
+                  min_score_result.score = tmp_score;
+                  min_score_result.transformation_matrix = transformation_score_matrix;
+                }
+              }
+            }
+          }
         }
       }
     }
@@ -101,6 +112,39 @@ matchingResult GridSearchMatching::gridSearch(
   return min_score_result;
 }
 
+Eigen::Matrix4d GridSearchMatching::getMatrix4d(const double & x, const double & y, const double & z,
+  const double & roll, const double & pitch, const double & yaw)
+{
+  Eigen::Matrix4d transformation_roll = Eigen::Matrix4d::Identity();
+  double phi = tier4_autoware_utils::deg2rad(roll);
+  transformation_roll(1, 1) = std::cos(phi);
+  transformation_roll(1, 2) = -sin(phi);
+  transformation_roll(2, 1) = sin(phi);
+  transformation_roll(2, 2) = std::cos(phi);
+
+  Eigen::Matrix4d transformation_pitch = Eigen::Matrix4d::Identity();
+  double theta = tier4_autoware_utils::deg2rad(pitch);
+  transformation_pitch(0, 0) = std::cos(theta);
+  transformation_pitch(0, 2) = sin(theta);
+  transformation_pitch(2, 0) = -sin(theta);
+  transformation_pitch(2, 2) = std::cos(theta);
+
+  Eigen::Matrix4d transformation_yaw = Eigen::Matrix4d::Identity();
+  double psi = tier4_autoware_utils::deg2rad(yaw);
+  transformation_yaw(0, 0) = std::cos(psi);
+  transformation_yaw(0, 1) = -sin(psi);
+  transformation_yaw(1, 0) = sin(psi);
+  transformation_yaw(1, 1) = std::cos(psi);
+
+  Eigen::Matrix4d transformation_score_matrix = Eigen::Matrix4d::Identity();
+  transformation_score_matrix = transformation_yaw * transformation_pitch * transformation_roll;
+  transformation_score_matrix(0, 3) = x;
+  transformation_score_matrix(1, 3) = y;
+  transformation_score_matrix(2, 3) = z;
+
+  return transformation_score_matrix;
+}
+
 std::vector<double> GridSearchMatching::generateSearchElement(
   const double & range_min, const double & range_max, const double & resolution)
 {