From c171d1a97ed64f0ac7c3727dd002d7ff2da5ad38 Mon Sep 17 00:00:00 2001
From: Benjamin Gilby <gilbybenjamin@gmail.com>
Date: Mon, 20 Nov 2023 17:10:32 +0900
Subject: [PATCH 1/6] make minimum changes to enable at128 launch

---
 .../config/centerpoint.param.yaml             | 15 ++++
 .../config/centerpoint_tiny.param.yaml        | 15 ++++
 .../detection_class_remapper.param.yaml       | 38 ++++++++++
 .../config/roi_sync.param.yaml                |  5 ++
 .../component/crop_box_filter.launch.xml      | 49 ++++++++++++
 .../lidar_camera_tf_publisher.launch.py       | 54 +++++++++++++
 .../component/lidar_centerpoint.launch.xml    | 32 ++++++++
 .../roi_detected_objects_fusion.launch.xml    | 33 ++++++++
 .../voxel_downsample_filter.launch.xml        | 17 +++++
 .../launch/perception_at128_sample.launch.xml | 76 +++++++++++++++++++
 10 files changed, 334 insertions(+)
 create mode 100644 edge_auto_jetson_launch/config/centerpoint.param.yaml
 create mode 100644 edge_auto_jetson_launch/config/centerpoint_tiny.param.yaml
 create mode 100644 edge_auto_jetson_launch/config/detection_class_remapper.param.yaml
 create mode 100644 edge_auto_jetson_launch/config/roi_sync.param.yaml
 create mode 100644 edge_auto_jetson_launch/launch/component/crop_box_filter.launch.xml
 create mode 100644 edge_auto_jetson_launch/launch/component/lidar_camera_tf_publisher.launch.py
 create mode 100644 edge_auto_jetson_launch/launch/component/lidar_centerpoint.launch.xml
 create mode 100644 edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
 create mode 100644 edge_auto_jetson_launch/launch/component/voxel_downsample_filter.launch.xml
 create mode 100644 edge_auto_jetson_launch/launch/perception_at128_sample.launch.xml

diff --git a/edge_auto_jetson_launch/config/centerpoint.param.yaml b/edge_auto_jetson_launch/config/centerpoint.param.yaml
new file mode 100644
index 0000000..fea3f3b
--- /dev/null
+++ b/edge_auto_jetson_launch/config/centerpoint.param.yaml
@@ -0,0 +1,15 @@
+/**:
+  ros__parameters:
+    class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
+    point_feature_size: 4
+    max_voxel_size: 40000
+    point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0]
+    voxel_size: [0.32, 0.32, 8.0]
+    downsample_factor: 1
+    encoder_in_feature_size: 9
+    # post-process params
+    circle_nms_dist_threshold: 0.5
+    iou_nms_target_class_names: ["CAR"]
+    iou_nms_search_distance_2d: 10.0
+    iou_nms_threshold: 0.1
+    yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0]
diff --git a/edge_auto_jetson_launch/config/centerpoint_tiny.param.yaml b/edge_auto_jetson_launch/config/centerpoint_tiny.param.yaml
new file mode 100644
index 0000000..92db4cd
--- /dev/null
+++ b/edge_auto_jetson_launch/config/centerpoint_tiny.param.yaml
@@ -0,0 +1,15 @@
+/**:
+  ros__parameters:
+    class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
+    point_feature_size: 4
+    max_voxel_size: 40000
+    point_cloud_range: [-76.8, -76.8, -2.0, 76.8, 76.8, 4.0]
+    voxel_size: [0.32, 0.32, 6.0]
+    downsample_factor: 2
+    encoder_in_feature_size: 9
+    # post-process params
+    circle_nms_dist_threshold: 0.5
+    iou_nms_target_class_names: ["CAR"]
+    iou_nms_search_distance_2d: 10.0
+    iou_nms_threshold: 0.1
+    yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0]
diff --git a/edge_auto_jetson_launch/config/detection_class_remapper.param.yaml b/edge_auto_jetson_launch/config/detection_class_remapper.param.yaml
new file mode 100644
index 0000000..ed378ff
--- /dev/null
+++ b/edge_auto_jetson_launch/config/detection_class_remapper.param.yaml
@@ -0,0 +1,38 @@
+/**:
+  ros__parameters:
+    allow_remapping_by_area_matrix:
+      # NOTE(kl): We turn all vehicles into trailers if they go over 3x12 [m^2].
+      # NOTE(kl): We turn cars into trucks if they have an area between 2.2 x 5.5 and 3.0 * 12.0 [m^2]
+      # row: original class. column: class to remap to
+      #UNKNOWN, CAR, TRUCK, BUS,  TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN
+      [0,       0,   0,     0,    0,       0,         0,      0,         #UNKNOWN
+       0,       0,   1,     0,    1,       0,         0,      0,         #CAR
+       0,       0,   0,     0,    1,       0,         0,      0,         #TRUCK
+       0,       0,   0,     0,    1,       0,         0,      0,         #BUS
+       0,       0,   0,     0,    0,       0,         0,      0,         #TRAILER
+       0,       0,   0,     0,    0,       0,         0,      0,         #MOTORBIKE
+       0,       0,   0,     0,    0,       0,         0,      0,         #BICYCLE
+       0,       0,   0,     0,    0,       0,         0,      0]         #PEDESTRIAN
+
+    min_area_matrix:
+      #UNKNOWN, CAR,   TRUCK,   BUS,   TRAILER,  MOTORBIKE, BICYCLE, PEDESTRIAN
+      [ 0.000,  0.000,  0.000,  0.000,  0.000,   0.000,     0.000,   0.000,      #UNKNOWN
+        0.000,  0.000, 12.100,  0.000, 36.000,   0.000,     0.000,   0.000,      #CAR
+        0.000,  0.000,  0.000,  0.000, 36.000,   0.000,     0.000,   0.000,      #TRUCK
+        0.000,  0.000,  0.000,  0.000, 36.000,   0.000,     0.000,   0.000,      #BUS
+        0.000,  0.000,  0.000,  0.000,  0.000,   0.000,     0.000,   0.000,      #TRAILER
+        0.000,  0.000,  0.000,  0.000,  0.000,   0.000,     0.000,   0.000,      #MOTORBIKE
+        0.000,  0.000,  0.000,  0.000,  0.000,   0.000,     0.000,   0.000,      #BICYCLE
+        0.000,  0.000,  0.000,  0.000,  0.000,   0.000,     0.000,   0.000]      #PEDESTRIAN
+
+
+    max_area_matrix:
+      #UNKNOWN, CAR,   TRUCK,  BUS,    TRAILER,   MOTORBIKE, BICYCLE, PEDESTRIAN
+      [ 0.000,  0.000,  0.000,  0.000,   0.000,   0.000,     0.000,   0.000,      #UNKNOWN
+        0.000,  0.000, 36.000,  0.000,     inf,   0.000,     0.000,   0.000,      #CAR
+        0.000,  0.000,  0.000,  0.000,     inf,   0.000,     0.000,   0.000,      #TRUCK
+        0.000,  0.000,  0.000,  0.000,     inf,   0.000,     0.000,   0.000,      #BUS
+        0.000,  0.000,  0.000,  0.000,   0.000,   0.000,     0.000,   0.000,      #TRAILER
+        0.000,  0.000,  0.000,  0.000,   0.000,   0.000,     0.000,   0.000,      #MOTORBIKE
+        0.000,  0.000,  0.000,  0.000,   0.000,   0.000,     0.000,   0.000,      #BICYCLE
+        0.000,  0.000,  0.000,  0.000,   0.000,   0.000,     0.000,   0.000]      #PEDESTRIAN
diff --git a/edge_auto_jetson_launch/config/roi_sync.param.yaml b/edge_auto_jetson_launch/config/roi_sync.param.yaml
new file mode 100644
index 0000000..364c9c5
--- /dev/null
+++ b/edge_auto_jetson_launch/config/roi_sync.param.yaml
@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    input_offset_ms: [50.0, 50.0]
+    timeout_ms: 70.0
+    match_threshold_ms: 50.0
diff --git a/edge_auto_jetson_launch/launch/component/crop_box_filter.launch.xml b/edge_auto_jetson_launch/launch/component/crop_box_filter.launch.xml
new file mode 100644
index 0000000..dcf4f05
--- /dev/null
+++ b/edge_auto_jetson_launch/launch/component/crop_box_filter.launch.xml
@@ -0,0 +1,49 @@
+<launch>
+  <arg name="input/pointcloud" default="input/pointcloud"/>
+  <arg name="output/pointcloud" default="output/pointcloud"/>
+  <arg name="frame_id" default="lidar"/>
+
+  <arg name="min_x" default="-100.0"/>
+  <arg name="min_y" default="-100.0"/>
+  <arg name="min_z" default="-100.0"/>
+  <arg name="max_x" default="100.0"/>
+  <arg name="max_y" default="100.0"/>
+  <arg name="max_z" default="100.0"/>
+  <arg name="negative" default="false" description="If true, points outside of the box will be published"/>
+  <arg name="container_name" default=""/>
+
+  <let name="empty_container_is_specified" value="$(eval 'not &quot;$(var container_name)&quot;')" />
+  <group if="$(var empty_container_is_specified)">
+    <node pkg="pointcloud_preprocessor" exec="crop_box_filter_node" name="crop_box_filter">
+      <remap from="input" to="$(var input/pointcloud)"/>
+      <remap from="output" to="$(var output/pointcloud)"/>
+      <param name="input_frame" value="$(var frame_id)"/>
+      <param name="output_frame" value="$(var frame_id)"/>
+      <param name="min_x" value="$(var min_x)"/>
+      <param name="min_y" value="$(var min_y)"/>
+      <param name="min_z" value="$(var min_z)"/>
+      <param name="max_x" value="$(var max_x)"/>
+      <param name="max_y" value="$(var max_y)"/>
+      <param name="max_z" value="$(var max_z)"/>
+      <param name="negative" value="$(var negative)"/>
+    </node>
+  </group>
+
+  <group unless="$(var empty_container_is_specified)">
+    <load_composable_node target="$(var container_name)">
+      <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::CropBoxFilterComponent" name="crop_box_filter">
+        <param name="input_frame" value="$(var frame_id)"/>
+        <param name="output_frame" value="$(var frame_id)"/>
+        <param name="min_x" value="$(var min_x)"/>
+        <param name="min_y" value="$(var min_y)"/>
+        <param name="min_z" value="$(var min_z)"/>
+        <param name="max_x" value="$(var max_x)"/>
+        <param name="max_y" value="$(var max_y)"/>
+        <param name="max_z" value="$(var max_z)"/>
+        <param name="negative" value="$(var negative)"/>
+        <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
+      </composable_node>
+    </load_composable_node>
+  </group>
+
+</launch>
diff --git a/edge_auto_jetson_launch/launch/component/lidar_camera_tf_publisher.launch.py b/edge_auto_jetson_launch/launch/component/lidar_camera_tf_publisher.launch.py
new file mode 100644
index 0000000..e5e3abf
--- /dev/null
+++ b/edge_auto_jetson_launch/launch/component/lidar_camera_tf_publisher.launch.py
@@ -0,0 +1,54 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+import json
+
+def launch_setup(context, *args, **kwargs):
+    tf_file = LaunchConfiguration('tf_file_path').perform(context)
+    with open(tf_file, 'r') as f:
+        tf_data = json.load(f)
+
+    return [
+        Node(
+            name="lidar_camera_tf_publisher",
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            arguments=[
+                '--x', str(tf_data['transform']['translation']['x']),
+                '--y', str(tf_data['transform']['translation']['y']),
+                '--z', str(tf_data['transform']['translation']['z']),
+                '--qx', str(tf_data['transform']['rotation']['x']),
+                '--qy', str(tf_data['transform']['rotation']['y']),
+                '--qz', str(tf_data['transform']['rotation']['z']),
+                '--qw', str(tf_data['transform']['rotation']['w']),
+                '--frame-id', str(tf_data['header']['frame_id']),
+                '--child-frame-id', str(tf_data['child_frame_id'])
+            ]),
+        Node(
+            name="camera_optical_link_publisher",
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            arguments=[
+                '--x', '0.0',
+                '--y', '0.0',
+                '--z', '0.0',
+                '--qx', '0.5',
+                '--qy', '-0.5',
+                '--qz', '0.5',
+                '--qw', '-0.5',
+                '--frame-id', str(tf_data['child_frame_id']),
+                '--child-frame-id', str(tf_data['child_frame_id'].replace('camera_link',
+                                                                          'camera_optical_link'))
+            ])
+    ]
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("tf_file_path"),
+            OpaqueFunction(function=launch_setup),
+        ]
+    )
+
diff --git a/edge_auto_jetson_launch/launch/component/lidar_centerpoint.launch.xml b/edge_auto_jetson_launch/launch/component/lidar_centerpoint.launch.xml
new file mode 100644
index 0000000..07e77ee
--- /dev/null
+++ b/edge_auto_jetson_launch/launch/component/lidar_centerpoint.launch.xml
@@ -0,0 +1,32 @@
+<launch>
+  <arg name="input/pointcloud" default="input/pointcloud"/>
+  <arg name="output/objects" default="output/objects"/>
+  <arg name="model_name" default="centerpoint_tiny" description="options: `centerpoint` or `centerpoint_tiny`"/>
+  <arg name="model_path" default="/opt/autoware/data/lidar_centerpoint/"/>
+  <arg name="model_param_path" default="$(find-pkg-share edge_auto_launch)/config/$(var model_name).param.yaml"/>
+  <arg name="class_remapper_param_path" default="$(find-pkg-share edge_auto_launch)/config/detection_class_remapper.param.yaml"/>
+  <arg name="score_threshold" default="0.35"/>
+  <arg name="densification_world_frame_id" default="map"/>
+  <arg name="densification_num_past_frames" default="1"/>
+  <arg name="has_twist" default="false"/>
+  <arg name="build_only" default="false" description="shutdown node after TensorRT engine file is built"/>
+
+  <group>
+    <node pkg="lidar_centerpoint" exec="lidar_centerpoint_node" name="lidar_centerpoint" output="screen">
+      <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
+      <remap from="~/output/objects" to="$(var output/objects)"/>
+      <param name="score_threshold" value="$(var score_threshold)"/>
+      <param name="densification_world_frame_id" value="$(var densification_world_frame_id)"/>
+      <param name="densification_num_past_frames" value="$(var densification_num_past_frames)"/>
+      <param name="trt_precision" value="fp16"/>
+      <param name="has_twist" value="$(var has_twist)"/>
+      <param name="encoder_onnx_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"/>
+      <param name="encoder_engine_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).engine"/>
+      <param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>
+      <param name="head_engine_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"/>
+      <param name="build_only" value="$(var build_only)"/>
+      <param from="$(var model_param_path)"/>
+      <param from="$(var class_remapper_param_path)"/>
+    </node>
+  </group>
+</launch>
diff --git a/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml b/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
new file mode 100644
index 0000000..bf00be6
--- /dev/null
+++ b/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
@@ -0,0 +1,33 @@
+<launch>
+  <arg name="input/rois_number" default="2"/>
+  <arg name="input/rois0" default="input/rois0"/>
+  <arg name="input/camera_info0" default="input/camera_info0"/>
+  <arg name="input/rois1" default="input/rois1"/>
+  <arg name="input/camera_info1" default="input/camera_info1"/>
+
+  <arg name="input/objects" default="input/objects"/>
+  <arg name="output/objects" default="output/objects"/>
+  <arg name="sync_param_path" default="$(find-pkg-share edge_auto_launch)/config/roi_sync.param.yaml"/>
+
+  <!-- for eval variable-->
+  <arg name="input_rois_number" default="$(var input/rois_number)"/>
+
+  <group>
+    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/rois0" to="$(var input/rois0)"/>
+    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/camera_info0" to="$(var input/camera_info0)"/>
+    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/rois1" to="$(var input/rois1)"/>
+    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/camera_info1" to="$(var input/camera_info1)"/>
+
+    <node pkg="image_projection_based_fusion" exec="roi_detected_object_fusion_node" name="roi_detected_object_fusion" output="screen">
+      <param name="rois_number" value="$(var input/rois_number)"/>
+      <param from="$(var sync_param_path)"/>
+      <remap from="input" to="$(var input/objects)"/>
+      <remap from="output" to="$(var output/objects)"/>
+      <param name="passthrough_lower_bound_probability_threshold" value="0.35"/>
+      <param name="use_roi_probability" value="false"/>
+      <param name="roi_probability_threshold" value="0.5"/>
+      <param name="min_iou_threshold" value="0.3"/>
+      <param name="debug_mode" value="false"/>
+    </node>
+  </group>
+</launch>
diff --git a/edge_auto_jetson_launch/launch/component/voxel_downsample_filter.launch.xml b/edge_auto_jetson_launch/launch/component/voxel_downsample_filter.launch.xml
new file mode 100644
index 0000000..a50b68d
--- /dev/null
+++ b/edge_auto_jetson_launch/launch/component/voxel_downsample_filter.launch.xml
@@ -0,0 +1,17 @@
+<launch>
+  <arg name="input/pointcloud" default="input/pointcloud"/>
+  <arg name="output/pointcloud" default="output/pointcloud"/>
+  <arg name="voxel_size_x" default="0.3"/>
+  <arg name="voxel_size_y" default="0.3"/>
+  <arg name="voxel_size_z" default="0.1"/>
+
+  <group>
+    <node pkg="pointcloud_preprocessor" exec="voxel_grid_downsample_filter_node" name="voxel_downsample_filter">
+      <remap from="input" to="$(var input/pointcloud)"/>
+      <remap from="output" to="$(var output/pointcloud)"/>
+      <param name="voxel_size_x" value="$(var voxel_size_x)"/>
+      <param name="voxel_size_y" value="$(var voxel_size_y)"/>
+      <param name="voxel_size_z" value="$(var voxel_size_z)"/>
+    </node>
+  </group>
+</launch>
diff --git a/edge_auto_jetson_launch/launch/perception_at128_sample.launch.xml b/edge_auto_jetson_launch/launch/perception_at128_sample.launch.xml
new file mode 100644
index 0000000..4ebb5d0
--- /dev/null
+++ b/edge_auto_jetson_launch/launch/perception_at128_sample.launch.xml
@@ -0,0 +1,76 @@
+<launch>
+  <!-- arguments -->
+  <arg name="vehicle_id" default="$(env VEHICLE_ID default)"/>
+  <arg name="lidar_correction_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/lidar/at128_default.dat"/>
+  <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)"/>
+  </include>
+  <include file="$(find-pkg-share edge_auto_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
+    <arg name="tf_file_path" value="$(var lidar_camera1_tf_file)"/>
+  </include>
+
+  <!-- driver -->
+  <group>
+    <push-ros-namespace namespace="/sensing/lidar/at128"/>
+    <include file="$(find-pkg-share edge_auto_launch)/launch/component/hesai_at128.launch.xml">
+      <arg name="frame_id" value="lidar"/>
+      <arg name="live_sensor" value="$(var live_sensor)"/>
+      <arg name="correction_file" value="$(var lidar_correction_file)"/>
+    </include>
+  </group>
+
+  <!-- perception -->
+  <group>
+    <push-ros-namespace namespace="/perception/object_recognition/detection"/>
+    <group>
+      <push-ros-namespace namespace="centerpoint"/>
+      <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="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"/>
+        <arg name="output/pointcloud" value="downsampled/pointcloud"/>
+        <arg name="voxel_size_x" value="0.3"/>
+        <arg name="voxel_size_y" value="0.3"/>
+        <arg name="voxel_size_z" value="0.1"/>
+      </include>
+      <include file="$(find-pkg-share edge_auto_launch)/launch/component/lidar_centerpoint.launch.xml">
+        <arg name="input/pointcloud" value="downsampled/pointcloud"/>
+        <arg name="output/objects" value="objects"/>
+        <arg name="model_name" value="centerpoint"/>
+        <arg name="score_threshold" value="0.075"/>
+        <arg name="densification_num_past_frames" value="0"/>
+      </include>
+    </group>
+    <group>
+      <push-ros-namespace namespace="image_projection_based_fusion"/>
+      <include file="$(find-pkg-share edge_auto_launch)/launch/component/roi_detected_objects_fusion.launch.xml">
+        <arg name="input/objects" value="/perception/object_recognition/detection/centerpoint/objects"/>
+        <arg name="output/objects" value="/perception/object_recognition/detection/objects"/>
+        <arg name="model_name" value="centerpoint"/>
+        <arg name="input/rois_number" value="2"/>
+        <arg name="input/rois0" value="/perception/object_recognition/detection/rois0"/>
+        <arg name="input/camera_info0" value="/sensing/camera/camera0/camera_info"/>
+        <arg name="input/rois1" value="/perception/object_recognition/detection/rois1"/>
+        <arg name="input/camera_info1" value="/sensing/camera/camera1/camera_info"/>
+      </include>
+    </group>
+  </group>
+</launch>

From 8efddfcbe49128641cf5a18f19f77a1cc5a3c508 Mon Sep 17 00:00:00 2001
From: Benjamin Gilby <gilbybenjamin@gmail.com>
Date: Tue, 21 Nov 2023 11:09:28 +0900
Subject: [PATCH 2/6] add extra deps

---
 edge_auto_jetson_launch/package.xml | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/edge_auto_jetson_launch/package.xml b/edge_auto_jetson_launch/package.xml
index 134b498..7f1bd08 100644
--- a/edge_auto_jetson_launch/package.xml
+++ b/edge_auto_jetson_launch/package.xml
@@ -19,6 +19,11 @@
   <exec_depend>bytetrack</exec_depend>
   <exec_depend>intrinsic_camera_calibrator</exec_depend>
   <exec_depend>image_transport_decompressor</exec_depend>
+  <exec_depend>image_projection_based_fusion</exec_depend>
+  <exec_depend>vehicle_info_util</exec_depend>
+  <exec_depend>lidar_centerpoint</exec_depend>
+  <exec_depend>nebula_sensor_driver</exec_depend>
+
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

From 5762bcf5ef0a8f2ab2ec66fa81f6b3678dab6a5b Mon Sep 17 00:00:00 2001
From: Benjamin Gilby <gilbybenjamin@gmail.com>
Date: Tue, 21 Nov 2023 11:35:58 +0900
Subject: [PATCH 3/6] add at128 launch

---
 .../launch/component/hesai_at128.launch.xml   | 75 +++++++++++++++++++
 1 file changed, 75 insertions(+)
 create mode 100755 edge_auto_jetson_launch/launch/component/hesai_at128.launch.xml

diff --git a/edge_auto_jetson_launch/launch/component/hesai_at128.launch.xml b/edge_auto_jetson_launch/launch/component/hesai_at128.launch.xml
new file mode 100755
index 0000000..b679985
--- /dev/null
+++ b/edge_auto_jetson_launch/launch/component/hesai_at128.launch.xml
@@ -0,0 +1,75 @@
+<launch>
+  <arg name="live_sensor" default="True"/>
+  <arg name="sensor_model" default="PandarAT128"/>
+  <arg name="return_mode" default="Strongest"/>
+  <arg name="frame_id" default="lidar"/>
+  <arg name="scan_phase" default="0.0"/>
+  <arg name="namespace" default=""/>
+  <arg name="calibration_file" default="$(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv"/>
+  <arg name="correction_file" default="$(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).dat"/>
+  <arg name="sensor_ip" default="192.168.1.201"/>
+  <arg name="host_ip" default="192.168.1.100"/>
+  <arg name="data_port" default="2368"/>
+  <arg name="gnss_port" default="10110"/>
+  <arg name="packet_mtu_size" default="1500"/>
+  <arg name="rotation_speed" default="200"/>
+  <arg name="cloud_min_angle" default="0"/>
+  <arg name="cloud_max_angle" default="360" />
+  <arg name="dual_return_distance_threshold" default="0.1"/>
+  <arg name="diag_span" default="1000"/>
+  <arg name="setup_sensor" default="True"/>
+
+  <group>
+    <node_container pkg="rclcpp_components" exec="component_container" name="nebula_at128_node" namespace="/">
+      <composable_node pkg="nebula_ros" plugin="HesaiDriverRosWrapper" name="hesai_driver_roswrapper_node" namespace="$(var namespace)">
+        <param name="sensor_model" value="$(var sensor_model)"/>
+        <param name="return_mode" value="$(var return_mode)"/>
+        <param name="frame_id" value="$(var frame_id)"/>
+        <param name="scan_phase" value="$(var scan_phase)"/>
+        <param name="calibration_file" value="$(var calibration_file)"/>
+        <param name="correction_file" value="$(var correction_file)"/>
+        <remap from="pandar_points" to="pointcloud_raw"/>
+        <extra_arg name="use_intra_process_comms" value="true"/>
+      </composable_node>
+    </node_container>
+
+    <load_composable_node target="nebula_at128_node" if="$(var live_sensor)">
+      <composable_node pkg="nebula_ros" plugin="HesaiHwInterfaceRosWrapper" name="hesai_hw_interface_ros_wrapper_node" namespace="$(var namespace)">
+        <param name="sensor_model" value="$(var sensor_model)"/>
+        <param name="return_mode" value="$(var return_mode)"/>
+        <param name="frame_id" value="$(var frame_id)"/>
+        <param name="scan_phase" value="$(var scan_phase)"/>
+        <param name="sensor_ip" value="$(var sensor_ip)"/>
+        <param name="frame_id" value="$(var frame_id)"/>
+        <param name="host_ip" value="$(var host_ip)"/>
+        <param name="data_port" value="$(var data_port)"/>
+        <param name="gnss_port" value="$(var gnss_port)"/>
+        <param name="packet_mtu_size" value="$(var packet_mtu_size)"/>
+        <param name="rotation_speed" value="$(var rotation_speed)"/>
+        <param name="cloud_min_angle" value="$(var cloud_min_angle)"/>
+        <param name="cloud_max_angle" value="$(var cloud_max_angle)"/>
+        <param name="dual_return_distance_threshold" value="$(var dual_return_distance_threshold)"/>
+        <param name="setup_sensor" value="$(var setup_sensor)"/>
+        <extra_arg name="use_intra_process_comms" value="true"/>
+      </composable_node>
+      <composable_node pkg="nebula_ros" plugin="HesaiHwMonitorRosWrapper" name="hesai_hw_monitor_ros_wrapper_node" namespace="$(var namespace)">
+        <param name="sensor_model" value="$(var sensor_model)"/>
+        <param name="return_mode" value="$(var return_mode)"/>
+        <param name="frame_id" value="$(var frame_id)"/>
+        <param name="scan_phase" value="$(var scan_phase)"/>
+        <param name="sensor_ip" value="$(var sensor_ip)"/>
+        <param name="frame_id" value="$(var frame_id)"/>
+        <param name="host_ip" value="$(var host_ip)"/>
+        <param name="data_port" value="$(var data_port)"/>
+        <param name="gnss_port" value="$(var gnss_port)"/>
+        <param name="packet_mtu_size" value="$(var packet_mtu_size)"/>
+        <param name="rotation_speed" value="$(var rotation_speed)"/>
+        <param name="cloud_min_angle" value="$(var cloud_min_angle)"/>
+        <param name="cloud_max_angle" value="$(var cloud_max_angle)"/>
+        <param name="diag_span" value="$(var diag_span)"/>
+        <param name="dual_return_distance_threshold" value="$(var dual_return_distance_threshold)"/>
+        <extra_arg name="use_intra_process_comms" value="true"/>
+      </composable_node>
+    </load_composable_node>
+  </group>
+</launch>

From e289f874b4ad007e1f30fdcfeab881ef39b9908f Mon Sep 17 00:00:00 2001
From: Benjamin Gilby <gilbybenjamin@gmail.com>
Date: Tue, 21 Nov 2023 13:04:56 +0900
Subject: [PATCH 4/6] modify references to edge_auto_launch

---
 .../launch/component/lidar_centerpoint.launch.xml  |  4 ++--
 .../roi_detected_objects_fusion.launch.xml         |  2 +-
 .../launch/perception_at128_sample.launch.xml      | 14 +++++++-------
 3 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/edge_auto_jetson_launch/launch/component/lidar_centerpoint.launch.xml b/edge_auto_jetson_launch/launch/component/lidar_centerpoint.launch.xml
index 07e77ee..0003a49 100644
--- a/edge_auto_jetson_launch/launch/component/lidar_centerpoint.launch.xml
+++ b/edge_auto_jetson_launch/launch/component/lidar_centerpoint.launch.xml
@@ -3,8 +3,8 @@
   <arg name="output/objects" default="output/objects"/>
   <arg name="model_name" default="centerpoint_tiny" description="options: `centerpoint` or `centerpoint_tiny`"/>
   <arg name="model_path" default="/opt/autoware/data/lidar_centerpoint/"/>
-  <arg name="model_param_path" default="$(find-pkg-share edge_auto_launch)/config/$(var model_name).param.yaml"/>
-  <arg name="class_remapper_param_path" default="$(find-pkg-share edge_auto_launch)/config/detection_class_remapper.param.yaml"/>
+  <arg name="model_param_path" default="$(find-pkg-share edge_auto_jetson_launch)/config/$(var model_name).param.yaml"/>
+  <arg name="class_remapper_param_path" default="$(find-pkg-share edge_auto_jetson_launch)/config/detection_class_remapper.param.yaml"/>
   <arg name="score_threshold" default="0.35"/>
   <arg name="densification_world_frame_id" default="map"/>
   <arg name="densification_num_past_frames" default="1"/>
diff --git a/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml b/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
index bf00be6..54ad68b 100644
--- a/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
+++ b/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
@@ -7,7 +7,7 @@
 
   <arg name="input/objects" default="input/objects"/>
   <arg name="output/objects" default="output/objects"/>
-  <arg name="sync_param_path" default="$(find-pkg-share edge_auto_launch)/config/roi_sync.param.yaml"/>
+  <arg name="sync_param_path" default="$(find-pkg-share edge_auto_jetson_launch)/config/roi_sync.param.yaml"/>
 
   <!-- for eval variable-->
   <arg name="input_rois_number" default="$(var input/rois_number)"/>
diff --git a/edge_auto_jetson_launch/launch/perception_at128_sample.launch.xml b/edge_auto_jetson_launch/launch/perception_at128_sample.launch.xml
index 4ebb5d0..6ca3ec7 100644
--- a/edge_auto_jetson_launch/launch/perception_at128_sample.launch.xml
+++ b/edge_auto_jetson_launch/launch/perception_at128_sample.launch.xml
@@ -16,17 +16,17 @@
   <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">
+  <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
     <arg name="tf_file_path" value="$(var lidar_camera0_tf_file)"/>
   </include>
-  <include file="$(find-pkg-share edge_auto_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
+  <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
     <arg name="tf_file_path" value="$(var lidar_camera1_tf_file)"/>
   </include>
 
   <!-- driver -->
   <group>
     <push-ros-namespace namespace="/sensing/lidar/at128"/>
-    <include file="$(find-pkg-share edge_auto_launch)/launch/component/hesai_at128.launch.xml">
+    <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/component/hesai_at128.launch.xml">
       <arg name="frame_id" value="lidar"/>
       <arg name="live_sensor" value="$(var live_sensor)"/>
       <arg name="correction_file" value="$(var lidar_correction_file)"/>
@@ -38,20 +38,20 @@
     <push-ros-namespace namespace="/perception/object_recognition/detection"/>
     <group>
       <push-ros-namespace namespace="centerpoint"/>
-      <include file="$(find-pkg-share edge_auto_launch)/launch/component/crop_box_filter.launch.xml">
+      <include file="$(find-pkg-share edge_auto_jetson_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="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">
+      <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/component/voxel_downsample_filter.launch.xml">
         <arg name="input/pointcloud" value="cropped/pointcloud"/>
         <arg name="output/pointcloud" value="downsampled/pointcloud"/>
         <arg name="voxel_size_x" value="0.3"/>
         <arg name="voxel_size_y" value="0.3"/>
         <arg name="voxel_size_z" value="0.1"/>
       </include>
-      <include file="$(find-pkg-share edge_auto_launch)/launch/component/lidar_centerpoint.launch.xml">
+      <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/component/lidar_centerpoint.launch.xml">
         <arg name="input/pointcloud" value="downsampled/pointcloud"/>
         <arg name="output/objects" value="objects"/>
         <arg name="model_name" value="centerpoint"/>
@@ -61,7 +61,7 @@
     </group>
     <group>
       <push-ros-namespace namespace="image_projection_based_fusion"/>
-      <include file="$(find-pkg-share edge_auto_launch)/launch/component/roi_detected_objects_fusion.launch.xml">
+      <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/component/roi_detected_objects_fusion.launch.xml">
         <arg name="input/objects" value="/perception/object_recognition/detection/centerpoint/objects"/>
         <arg name="output/objects" value="/perception/object_recognition/detection/objects"/>
         <arg name="model_name" value="centerpoint"/>

From 502c4a506c84b822b8ac1c10db6ee4be999df7ef Mon Sep 17 00:00:00 2001
From: Benjamin Gilby <gilbybenjamin@gmail.com>
Date: Tue, 21 Nov 2023 13:15:36 +0900
Subject: [PATCH 5/6] add updated roi detection objects fusion params

---
 .../config/can_assign_matrix.yaml             | 70 +++++++++++++++++++
 .../roi_detected_objects_fusion.launch.xml    |  5 +-
 2 files changed, 74 insertions(+), 1 deletion(-)
 create mode 100644 edge_auto_jetson_launch/config/can_assign_matrix.yaml

diff --git a/edge_auto_jetson_launch/config/can_assign_matrix.yaml b/edge_auto_jetson_launch/config/can_assign_matrix.yaml
new file mode 100644
index 0000000..fb73dc7
--- /dev/null
+++ b/edge_auto_jetson_launch/config/can_assign_matrix.yaml
@@ -0,0 +1,70 @@
+/**:
+  ros__parameters:
+    can_assign_matrix:
+      #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN <-roi_msg
+      [
+        1,
+        0,
+        0,
+        0,
+        0,
+        0,
+        0,
+        0, # UNKNOWN <-detected_objects
+        0,
+        1,
+        0,
+        0,
+        0,
+        0,
+        0,
+        0, # CAR
+        0,
+        0,
+        1,
+        0,
+        0,
+        0,
+        0,
+        0, # TRUCK
+        0,
+        0,
+        0,
+        1,
+        0,
+        0,
+        0,
+        0, # BUS
+        0,
+        0,
+        0,
+        0,
+        1,
+        0,
+        0,
+        0, # TRAILER
+        0,
+        0,
+        0,
+        0,
+        0,
+        1,
+        0,
+        0, # MOTORBIKE
+        0,
+        0,
+        0,
+        0,
+        0,
+        0,
+        1,
+        0, # BICYCLE
+        0,
+        0,
+        0,
+        0,
+        0,
+        0,
+        0,
+        1,
+      ] # PEDESTRIAN
diff --git a/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml b/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
index 54ad68b..27c5b32 100644
--- a/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
+++ b/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
@@ -8,6 +8,7 @@
   <arg name="input/objects" default="input/objects"/>
   <arg name="output/objects" default="output/objects"/>
   <arg name="sync_param_path" default="$(find-pkg-share edge_auto_jetson_launch)/config/roi_sync.param.yaml"/>
+  <arg name="can_assign_matrix_param_path" default="$(find-pkg-share edge_auto_launch)/config/can_assign_matrix.yaml"/>
 
   <!-- for eval variable-->
   <arg name="input_rois_number" default="$(var input/rois_number)"/>
@@ -23,11 +24,13 @@
       <param from="$(var sync_param_path)"/>
       <remap from="input" to="$(var input/objects)"/>
       <remap from="output" to="$(var output/objects)"/>
-      <param name="passthrough_lower_bound_probability_threshold" value="0.35"/>
+      <param name="passthrough_lower_bound_probability_thresholds" value="[0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35]"/>
       <param name="use_roi_probability" value="false"/>
       <param name="roi_probability_threshold" value="0.5"/>
       <param name="min_iou_threshold" value="0.3"/>
       <param name="debug_mode" value="false"/>
+      <param name="trust_distances" value="[100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0]"/>
+      <param from="$(var can_assign_matrix_param_path)"/>
     </node>
   </group>
 </launch>

From 168a561a59d9d4234b439be95ddf8416bff9608f Mon Sep 17 00:00:00 2001
From: Benjamin Gilby <gilbybenjamin@gmail.com>
Date: Tue, 21 Nov 2023 13:17:00 +0900
Subject: [PATCH 6/6] modify reference to edge_auto_launch

---
 .../launch/component/roi_detected_objects_fusion.launch.xml     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml b/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
index 27c5b32..7ef982a 100644
--- a/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
+++ b/edge_auto_jetson_launch/launch/component/roi_detected_objects_fusion.launch.xml
@@ -8,7 +8,7 @@
   <arg name="input/objects" default="input/objects"/>
   <arg name="output/objects" default="output/objects"/>
   <arg name="sync_param_path" default="$(find-pkg-share edge_auto_jetson_launch)/config/roi_sync.param.yaml"/>
-  <arg name="can_assign_matrix_param_path" default="$(find-pkg-share edge_auto_launch)/config/can_assign_matrix.yaml"/>
+  <arg name="can_assign_matrix_param_path" default="$(find-pkg-share edge_auto_jetson_launch)/config/can_assign_matrix.yaml"/>
 
   <!-- for eval variable-->
   <arg name="input_rois_number" default="$(var input/rois_number)"/>