fix: xt32 sample (#10)

* fix: coodinate system for xt32 sample Signed-off-by: Akihito Ohsato <[email protected]> * feat(perception_xt32_sample): add fusion function Signed-off-by: Manato HIRABAYASHI <[email protected]> * feat(perception_xt32_sample): adjust RViz style to the AT128 one Signed-off-by: Manato HIRABAYASHI <[email protected]> --------- Signed-off-by: Akihito Ohsato <[email protected]> Signed-off-by: Manato HIRABAYASHI <[email protected]> Co-authored-by: Akihito Ohsato <[email protected]>
tier4 · Sep 29, 2023 · 4806b8e · 4806b8e
1 parent 91e273e
commit 4806b8e
Show file tree

Hide file tree

Showing 3 changed files with 140 additions and 26 deletions.
diff --git a/edge_auto_launch/config/visualization/perception_xt32_sample.rviz b/edge_auto_launch/config/visualization/perception_xt32_sample.rviz
@@ -1,11 +1,12 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 138
     Name: Displays
     Property Tree Widget:
-      Expanded: ~
-      Splitter Ratio: 0.48444443941116333
-    Tree Height: 719
+      Expanded:
+        - /Pointcloud1
+      Splitter Ratio: 0.34330984950065613
+    Tree Height: 1740
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -100,6 +101,56 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - BUS:
+        Alpha: 0.30000001192092896
+        Color: 255; 255; 255
+      CAR:
+        Alpha: 0.30000001192092896
+        Color: 255; 255; 255
+      CYCLIST:
+        Alpha: 0.30000001192092896
+        Color: 255; 255; 255
+      Class: autoware_auto_perception_rviz_plugin/DetectedObjects
+      Display Acceleration: true
+      Display Label: true
+      Display PoseWithCovariance: true
+      Display Predicted Path Confidence: true
+      Display Predicted Paths: true
+      Display Twist: true
+      Display UUID: true
+      Display Velocity: true
+      Enabled: true
+      Line Width: 0.019999999552965164
+      MOTORCYCLE:
+        Alpha: 0.30000001192092896
+        Color: 255; 255; 255
+      Name: EuclideanCluster
+      Namespaces:
+        label: true
+        shape: true
+        twist: true
+        velocity: true
+      PEDESTRIAN:
+        Alpha: 0.30000001192092896
+        Color: 255; 255; 255
+      Polygon Type: 3d
+      TRAILER:
+        Alpha: 0.30000001192092896
+        Color: 255; 255; 255
+      TRUCK:
+        Alpha: 0.30000001192092896
+        Color: 255; 255; 255
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /perception/object_recognition/detection/euclidean_cluster/objects
+      UNKNOWN:
+        Alpha: 0.30000001192092896
+        Color: 255; 255; 255
+      Value: true
+      Visualization Type: Normal
     - BUS:
         Alpha: 1
         Color: 30; 144; 255
@@ -123,7 +174,7 @@ Visualization Manager:
       MOTORCYCLE:
         Alpha: 1
         Color: 30; 144; 255
-      Name: Detected Objects
+      Name: Fusion
       Namespaces:
         label: true
         shape: true
@@ -196,41 +247,41 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 11.199999809265137
+      Distance: 17.369640350341797
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.20143486559391022
-        Y: -0.5514689087867737
-        Z: 0.7952755689620972
+        X: -0.3781598210334778
+        Y: -0.7969066500663757
+        Z: 2.003519058227539
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.560398519039154
+      Pitch: 0.5603983998298645
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz_default_plugins)
-      Yaw: 1.7753984928131104
+      Yaw: 1.455397605895996
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1016
+  Height: 2246
   Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001c40000035afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002cf000000c70000000000000000000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004570000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000002e5000007c4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed00000a8000000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000007c40000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002cf000000c70000000000000000000000010000033200001056fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000010560000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000fb60000005afc0100000002fb0000000800540069006d0065010000000000000fb60000058100fffffffb0000000800540069006d0065010000000000000450000000000000000000000cc5000007c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
-  Width: 1846
-  X: 74
-  Y: 27
+    collapsed: true
+  Width: 4022
+  X: 696
+  Y: 3227
diff --git a/edge_auto_launch/launch/component/roi_cluster_fusion.launch.xml b/edge_auto_launch/launch/component/roi_cluster_fusion.launch.xml
@@ -0,0 +1,42 @@
+<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/clusters" default="input/clusters"/>
+  <arg name="output/clusters" default="output/clusters"/>
+  <arg name="sync_param_path" default="$(find-pkg-share edge_auto_launch)/config/roi_sync.param.yaml"/>
+  <arg name="remove_unknown" default="true"/>
+  <arg name="trust_distance" default="100.0"/>
+
+  <!-- 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_cluster_fusion_node" name="roi_cluster_fusion" output="screen">
+      <param name="rois_number" value="$(var input/rois_number)"/>
+      <param from="$(var sync_param_path)"/>
+      <remap from="input" to="$(var input/clusters)"/>
+      <remap from="output" to="$(var output/clusters)"/>
+
+      <param name="remove_unknown" value="$(var remove_unknown)"/>
+      <param name="trust_distance" value="$(var trust_distance)"/>
+
+      <param name="use_iou" value="true"/>
+      <param name="use_iou_x" value="false"/>
+      <param name="use_iou_y" value="false"/>
+      <param name="only_allow_inside_cluster" value="true"/>
+      <param name="roi_scale_factor" value="1.1"/>
+
+      <param name="iou_threshold" value="0.3"/>
+      <param name="debug_mode" value="false"/>
+    </node>
+  </group>
+</launch>
diff --git a/edge_auto_launch/launch/perception_xt32_sample.launch.xml b/edge_auto_launch/launch/perception_xt32_sample.launch.xml
@@ -34,13 +34,8 @@
       <include file="$(find-pkg-share edge_auto_launch)/launch/component/crop_box_filter.launch.xml">
         <arg name="input/pointcloud" value="/sensing/lidar/xt32/pointcloud_raw"/>
         <arg name="output/pointcloud" value="cropped/pointcloud"/>
-        <arg name="min_x" value="-0.15"/>
-        <arg name="min_y" value="-0.15"/>
-        <arg name="min_z" value="-0.15"/>
-        <arg name="max_x" value="0.15"/>
-        <arg name="max_y" value="0.15"/>
-        <arg name="max_z" value="0.15"/>
-        <arg name="negative" value="true"/>
+        <arg name="frame_id" value="base_link"/>
+        <arg name="max_z" value="1.2"/>
       </include>
       <push-ros-namespace namespace="obstacle_segmentation"/>
       <include file="$(find-pkg-share edge_auto_launch)/launch/component/scan_ground_filter.launch.xml">
@@ -59,15 +54,41 @@
         <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
           <arg name="input/objects" value="clusters"/>
           <arg name="output/objects" value="objects_with_feature"/>
+          <arg name="node_name" value="shape_estimation"/>
         </include>
         <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
           <arg name="input" value="objects_with_feature"/>
-          <arg name="output" value="/perception/object_recognition/detection/objects"/>
+          <arg name="output" value="/perception/object_recognition/detection/euclidean_cluster/objects"/>
+          <arg name="node_name" value="detected_object_feature_remover"/>
         </include>
       </group>
     </group>
+    <group>
+      <push-ros-namespace namespace="image_projection_based_fusion"/>
+      <include file="$(find-pkg-share edge_auto_launch)/launch/component/roi_cluster_fusion.launch.xml">
+        <arg name="input/clusters" value="/perception/object_recognition/detection/euclidean_cluster/clusters"/>
+        <arg name="output/clusters" value="clusters"/>
+        <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>
+      <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
+        <arg name="input/objects" value="clusters"/>
+        <arg name="output/objects" value="objects_with_feature"/>
+        <arg name="node_name" value="shape_estimation"/>
+      </include>
+      <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
+        <arg name="input" value="objects_with_feature"/>
+        <arg name="output" value="/perception/object_recognition/detection/objects"/>
+        <arg name="node_name" value="detected_object_feature_remover"/>
+      </include>
+    </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"/>