From a37c130d1877d34ea44f16f002607a783046c5b4 Mon Sep 17 00:00:00 2001
From: Manato HIRABAYASHI <manato.hirabayashi@tier4.jp>
Date: Wed, 19 Jul 2023 21:41:15 +0900
Subject: [PATCH] feat: introduce base_link and add `sensor_height` parameter

Signed-off-by: Manato HIRABAYASHI <manato.hirabayashi@tier4.jp>
---
 .../launch/perception_at128_sample.launch.xml             | 7 ++++++-
 edge_auto_launch/launch/perception_xt32_sample.launch.xml | 8 +++++---
 2 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/edge_auto_launch/launch/perception_at128_sample.launch.xml b/edge_auto_launch/launch/perception_at128_sample.launch.xml
index b9fd57d..382ac36 100644
--- a/edge_auto_launch/launch/perception_at128_sample.launch.xml
+++ b/edge_auto_launch/launch/perception_at128_sample.launch.xml
@@ -5,12 +5,16 @@
   <arg name="lidar_camera0_tf_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/at128_to_camera0.json"/>
   <arg name="lidar_camera1_tf_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/at128_to_camera1.json"/>
   <arg name="live_sensor" default="True"/>
+  <arg name="sensor_height" default="1.0"/>
 
   <!-- vehicle_info -->
   <include file="$(find-pkg-share vehicle_info_util)/launch/vehicle_info.launch.py">
     <arg name="vehicle_info_param_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/vehicle_info.param.yaml"/>
   </include>
 
+  <!-- base_link generation -->
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z $(var sensor_height) --qx 0.0 --qy 0.0 --qz 0.0 --qw 1.0 --frame-id base_link --child-frame-id lidar" />
+
   <!-- calibration -->
   <include file="$(find-pkg-share edge_auto_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
     <arg name="tf_file_path" value="$(var lidar_camera0_tf_file)"/>
@@ -37,7 +41,8 @@
       <include file="$(find-pkg-share edge_auto_launch)/launch/component/crop_box_filter.launch.xml">
         <arg name="input/pointcloud" value="/sensing/lidar/at128/pointcloud_raw"/>
         <arg name="output/pointcloud" value="cropped/pointcloud"/>
-        <arg name="max_z" value="1.2"/>
+        <arg name="frame_id" value="base_link"/>
+        <arg name="max_z" value="2.0"/>
       </include>
       <include file="$(find-pkg-share edge_auto_launch)/launch/component/voxel_downsample_filter.launch.xml">
         <arg name="input/pointcloud" value="cropped/pointcloud"/>
diff --git a/edge_auto_launch/launch/perception_xt32_sample.launch.xml b/edge_auto_launch/launch/perception_xt32_sample.launch.xml
index 945b6f6..b49644f 100755
--- a/edge_auto_launch/launch/perception_xt32_sample.launch.xml
+++ b/edge_auto_launch/launch/perception_xt32_sample.launch.xml
@@ -4,12 +4,16 @@
   <arg name="lidar_camera0_tf_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/xt32_to_camera0.json"/>
   <arg name="lidar_camera1_tf_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/xt32_to_camera1.json"/>
   <arg name="live_sensor" default="True"/>
+  <arg name="sensor_height" default="1.0"/>
 
   <!-- vehicle_info -->
   <include file="$(find-pkg-share vehicle_info_util)/launch/vehicle_info.launch.py">
     <arg name="vehicle_info_param_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/vehicle_info.param.yaml"/>
   </include>
 
+  <!-- base_link generation -->
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z $(var sensor_height) --qx 0.0 --qy 0.0 --qz 0.0 --qw 1.0 --frame-id base_link --child-frame-id lidar" />
+
   <!-- calibration -->
   <include file="$(find-pkg-share edge_auto_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
     <arg name="tf_file_path" value="$(var lidar_camera0_tf_file)"/>
@@ -35,7 +39,7 @@
         <arg name="input/pointcloud" value="/sensing/lidar/xt32/pointcloud_raw"/>
         <arg name="output/pointcloud" value="cropped/pointcloud"/>
         <arg name="frame_id" value="base_link"/>
-        <arg name="max_z" value="1.2"/>
+        <arg name="max_z" value="2.0"/>
       </include>
       <push-ros-namespace namespace="obstacle_segmentation"/>
       <include file="$(find-pkg-share edge_auto_launch)/launch/component/scan_ground_filter.launch.xml">
@@ -87,8 +91,6 @@
     </group>
   </group>
 
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 1.0 --qx 0.0 --qy 0.0 --qz 0.0 --qw 1.0 --frame-id base_link --child-frame-id lidar" />
-
   <!-- visualization -->
   <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share edge_auto_launch)/config/visualization/perception_xt32_sample.rviz"/>
   <node pkg="rqt_gui" exec="rqt_gui" name="rqt_gui" output="screen" args="--perspective-file $(find-pkg-share edge_auto_launch)/config/visualization/perception_xt32_sample.perspective"/>