From a7c7169d12cac4ca669468688624a3cd81181809 Mon Sep 17 00:00:00 2001
From: kminoda <44218668+kminoda@users.noreply.github.com>
Date: Thu, 27 Apr 2023 10:52:22 +0900
Subject: [PATCH] feat(pcdless_launch): add multi camera launcher (#22)

* feat(pcdless_launch): add multi camera launcher

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* minor fix

Signed-off-by: kminoda <koji.minoda@tier4.jp>

---------

Signed-off-by: kminoda <koji.minoda@tier4.jp>
---
 .../launch/pcdless_multi_camera.launch.xml    | 101 ++++++++++++++++++
 .../launch/tier4_jpntaxi.launch.xml           |  31 ++++++
 .../launch/tier4_jpntaxi_multi.launch.xml     |  33 ++++++
 3 files changed, 165 insertions(+)
 create mode 100644 localization/yabloc/pcdless_launch/launch/pcdless_multi_camera.launch.xml
 create mode 100644 localization/yabloc/pcdless_launch/launch/tier4_jpntaxi.launch.xml
 create mode 100644 localization/yabloc/pcdless_launch/launch/tier4_jpntaxi_multi.launch.xml

diff --git a/localization/yabloc/pcdless_launch/launch/pcdless_multi_camera.launch.xml b/localization/yabloc/pcdless_launch/launch/pcdless_multi_camera.launch.xml
new file mode 100644
index 0000000000000..8bd82545660d4
--- /dev/null
+++ b/localization/yabloc/pcdless_launch/launch/pcdless_multi_camera.launch.xml
@@ -0,0 +1,101 @@
+<launch>
+    <!-- NOTE: -->
+    <arg name="standalone" description="[true,false] Set to true if not connected to Autoware's P/C."/>
+    <arg name="use_sim_time" default="true"/>
+    <arg name="use_septentrio" default="false" description="septentrio gnss"/>
+
+    <!-- source camera image topics -->
+    <arg name="src_image_0" default="/sensing/camera/camera0/image_rect_color/compressed"/>
+    <arg name="src_info_0" default="/sensing/camera/camera0/camera_info"/>
+    <arg name="src_image_1" default="/sensing/camera/camera1/image_rect_color/compressed"/>
+    <arg name="src_info_1" default="/sensing/camera/camera1/camera_info"/>
+    <arg name="resized_image_0" default="/sensing/camera/undistorted_0/image_raw"/>
+    <arg name="resized_info_0" default="/sensing/camera/undistorted_0/camera_info"/>
+    <arg name="resized_image_1" default="/sensing/camera/undistorted_1/image_raw"/>
+    <arg name="resized_info_1" default="/sensing/camera/undistorted_1/camera_info"/>
+
+    <let name="connect_base_link_to_particle_pose" value="true" if="$(var standalone)"/>
+    <let name="connect_base_link_to_particle_pose" value="false" unless="$(var standalone)"/>
+    <let name="input_pose" value="/localization/pf/pose" if="$(var standalone)"/>
+    <let name="input_pose" value="/localization/pose_twist_fusion_filter/pose" unless="$(var standalone)"/>
+
+    <group>
+        <push-ros-namespace namespace="localization"/>
+
+        <!-- initializer -->
+        <group>
+            <push-ros-namespace namespace="initializer"/>
+            <include file="$(find-pkg-share pcdless_launch)/launch/impl/initializer.launch.xml">
+                <arg name="skip_autoware_pose_initializer" value="$(var standalone)"/>
+            </include>
+        </group>
+
+        <!-- particle filter -->
+        <group>
+            <push-ros-namespace namespace="pf"/>
+            <include file="$(find-pkg-share pcdless_launch)/launch/impl/pf.launch.xml"/>
+        </group>
+
+        <!-- static tf -->
+        <group if="$(var connect_base_link_to_particle_pose)"> 
+            <node name="base_link_tf" pkg="tf2_ros" exec="static_transform_publisher" args="--frame-id /particle_filter --child-frame-id /base_link"/>
+        </group>
+
+        <!-- image processing -->
+        <group>
+            <push-ros-namespace namespace="imgproc/camera0"/>
+            <include file="$(find-pkg-share pcdless_launch)/launch/impl/imgproc.launch.xml">
+                <arg name="src_image" value="$(var src_image_0)"/>
+                <arg name="src_info" value="$(var src_info_0)"/>
+                <arg name="resized_image" value="$(var resized_image_0)"/>
+                <arg name="resized_info" value="$(var resized_info_0)"/>
+            </include>
+        </group>
+        <group>
+            <push-ros-namespace namespace="imgproc/camera1"/>
+            <include file="$(find-pkg-share pcdless_launch)/launch/impl/imgproc.launch.xml">
+                <arg name="src_image" value="$(var src_image_1)"/>
+                <arg name="src_info" value="$(var src_info_1)"/>
+                <arg name="resized_image" value="$(var resized_image_1)"/>
+                <arg name="resized_info" value="$(var resized_info_1)"/>
+            </include>
+        </group>
+
+        <!-- twist -->
+        <group>
+            <push-ros-namespace namespace="twist"/>
+            <include file="$(find-pkg-share pcdless_launch)/launch/impl/twist.launch.xml"/>
+        </group>
+
+        <!-- validation -->
+        <group>
+            <push-ros-namespace namespace="validation/camera0"/>
+            <include file="$(find-pkg-share pcdless_launch)/launch/impl/validation.launch.xml">
+                <arg name="input_overlayed_pose" value="$(var input_pose)"/>
+                <arg name="resized_image" value="$(var resized_image_0)"/>
+                <arg name="resized_info" value="$(var resized_info_0)"/>
+            </include>
+        </group>
+        <group>
+            <push-ros-namespace namespace="validation/camera1"/>
+            <include file="$(find-pkg-share pcdless_launch)/launch/impl/validation.launch.xml">
+                <arg name="input_overlayed_pose" value="$(var input_pose)"/>
+                <arg name="resized_image" value="$(var resized_image_1)"/>
+                <arg name="resized_info" value="$(var resized_info_1)"/>
+            </include>
+        </group>
+
+        <!-- map -->
+        <group>
+            <push-ros-namespace namespace="map"/>
+            <include file="$(find-pkg-share pcdless_launch)/launch/impl/map.launch.xml">
+                <arg name="input_particle_pose" value="$(var input_pose)"/>
+            </include>
+        </group>
+
+        <!-- septentrio ublox converter -->
+        <group if="$(var use_septentrio)">
+            <node name="doppler_converter" pkg="doppler_converter" exec="doppler_converter"/>
+        </group>
+    </group>
+</launch>
\ No newline at end of file
diff --git a/localization/yabloc/pcdless_launch/launch/tier4_jpntaxi.launch.xml b/localization/yabloc/pcdless_launch/launch/tier4_jpntaxi.launch.xml
new file mode 100644
index 0000000000000..fcbb96ae69339
--- /dev/null
+++ b/localization/yabloc/pcdless_launch/launch/tier4_jpntaxi.launch.xml
@@ -0,0 +1,31 @@
+<launch>
+    <arg name="standalone" default="true" description="Set to true if not connected to Autoware's P/C."/>
+
+    <include file="$(find-pkg-share pcdless_launch)/launch/pcdless.launch.xml">
+        <arg name="standalone" value="$(var standalone)"/>
+
+        <arg name="min_segment_length" value="1.5"/>
+        <arg name="max_segment_distance" value="30.0"/>
+        <arg name="ignore_less_than_float" value="true"/>
+
+        <arg name="static_linear_covariance" value="0.04"/>
+        <arg name="static_angular_covariance" value="0.0010"/>
+        <arg name="static_scale_factor" value="1.00"/>
+        <arg name="static_gyro_bias" value="10.0"/>
+
+        <arg name="road_marking_labels" value="[cross_walk, zebra_marking, line_thin, line_thick, pedestrian_marking, stop_line, road_border]"/>
+        <arg name="sign_board_labels" value="[sign-board]"/>
+
+        <arg name="src_image" value="/sensing/camera/camera0/image_rect_color/compressed"/>
+        <arg name="src_info" value="/sensing/camera/camera0/camera_info"/>
+
+        <arg name="target_height_ratio" value="0.80"/>
+        <!-- <arg name="override_camera_frame_id" value="fake_camera_optical_link"/> -->
+        <arg name="use_septentrio" value="true"/>
+    </include>
+
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="fake_tf1" args="--frame-id base_link                 --child-frame-id fake_sensor_kit_base_link --x 0.6 --z 1.7 --yaw 0.03" />
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="fake_tf2" args="--frame-id fake_sensor_kit_base_link --child-frame-id fake_camera_link          --x 0.8703194 --y 0.01868 --z 0.09616 --roll -0.000664 --pitch 0.0085596 --yaw -0.01503"/>
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="fake_tf3" args="--frame-id fake_camera_link          --child-frame-id fake_camera_optical_link  --roll -1.57 --yaw -1.57" />
+
+</launch>
\ No newline at end of file
diff --git a/localization/yabloc/pcdless_launch/launch/tier4_jpntaxi_multi.launch.xml b/localization/yabloc/pcdless_launch/launch/tier4_jpntaxi_multi.launch.xml
new file mode 100644
index 0000000000000..cbb15ba6160a6
--- /dev/null
+++ b/localization/yabloc/pcdless_launch/launch/tier4_jpntaxi_multi.launch.xml
@@ -0,0 +1,33 @@
+<launch>
+    <arg name="standalone" default="true" description="Set to true if not connected to Autoware's P/C."/>
+
+    <include file="$(find-pkg-share pcdless_launch)/launch/pcdless_multi_camera.launch.xml">
+        <arg name="standalone" value="$(var standalone)"/>
+
+        <arg name="min_segment_length" value="1.5"/>
+        <arg name="max_segment_distance" value="30.0"/>
+        <arg name="ignore_less_than_float" value="true"/>
+
+        <arg name="static_linear_covariance" value="0.04"/>
+        <arg name="static_angular_covariance" value="0.0010"/>
+        <arg name="static_scale_factor" value="1.00"/>
+        <arg name="static_gyro_bias" value="10.0"/>
+
+        <arg name="road_marking_labels" value="[cross_walk, zebra_marking, line_thin, line_thick, pedestrian_marking, stop_line, road_border]"/>
+        <arg name="sign_board_labels" value="[sign-board]"/>
+
+        <arg name="src_image_0" value="/sensing/camera/camera0/image_rect_color/compressed"/>
+        <arg name="src_info_0" value="/sensing/camera/camera0/camera_info"/>
+        <arg name="src_image_1" value="/sensing/camera/camera1/image_rect_color/compressed"/>
+        <arg name="src_info_1" value="/sensing/camera/camera1/camera_info"/>
+
+        <arg name="target_height_ratio" value="0.80"/>
+        <!-- <arg name="override_camera_frame_id" value="fake_camera_optical_link"/> -->
+        <arg name="use_septentrio" value="true"/>
+    </include>
+
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="fake_tf1" args="--frame-id base_link                 --child-frame-id fake_sensor_kit_base_link --x 0.6 --z 1.7 --yaw 0.03" />
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="fake_tf2" args="--frame-id fake_sensor_kit_base_link --child-frame-id fake_camera_link          --x 0.8703194 --y 0.01868 --z 0.09616 --roll -0.000664 --pitch 0.0085596 --yaw -0.01503"/>
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="fake_tf3" args="--frame-id fake_camera_link          --child-frame-id fake_camera_optical_link  --roll -1.57 --yaw -1.57" />
+
+</launch>
\ No newline at end of file