feat: add detected object validator (autowarefoundation#250)

* feat: add detected object validator Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * bug fix Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * bug fix * bug fix
TomohitoAndo · Mar 30, 2022 · 8c7f591 · 8c7f591
1 parent 7e04bc0
commit 8c7f591
Show file tree

Hide file tree

Showing 4 changed files with 63 additions and 11 deletions.
diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -17,8 +17,9 @@
   <arg name="image_raw7" default="/image_raw7"/>
   <arg name="camera_info7" default="/camera_info7"/>
   <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
-  <arg name="use_vector_map" default="true" description="use vector map in prediction"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
+  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+  <arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
@@ -33,9 +34,22 @@
     <arg name="image_number" value="$(var image_number)"/>
   </include> -->
 
+  <!-- Pointcloud map filter -->
+  <group>
+    <include file="$(find-pkg-share compare_map_segmentation)/launch/voxel_based_compare_map_filter.launch.xml" if="$(var use_pointcloud_map)">
+      <arg name="input" value="$(var input_pointcloud)" />
+      <arg name="input_map" value="/map/pointcloud_map" />
+      <arg name="output" value="pointcloud_map_filtered/pointcloud" />
+      <arg name="distance_threshold" value="0.5" />
+    </include>
+  </group>
+
   <group>
     <push-ros-namespace namespace="clustering"/>
+    <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
+    <let name="clustering/input/pointcloud" value="$(var input_pointcloud)" unless="$(var use_pointcloud_map)"/>
     <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
+      <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
       <arg name="output_clusters" value="clusters"/>
       <arg name="use_pointcloud_map" value="false"/>
     </include>
@@ -85,6 +99,13 @@
     </include>
   </group>
 
+  <!-- DetectionByTracker -->
+  <group>
+    <push-ros-namespace namespace="detection_by_tracker"/>
+    <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml">
+    </include>
+  </group>
+
   <!-- CenterPoint -->
   <group if="$(eval &quot;'$(var lidar_detection_model)'=='centerpoint'&quot;)">
     <push-ros-namespace namespace="centerpoint"/>
@@ -110,16 +131,20 @@
     </group>
   </group>
 
-  <!-- DetectionByTracker -->
+  <!-- Validator -->
   <group>
-    <push-ros-namespace namespace="detection_by_tracker"/>
-    <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml">
+    <let name="validator/input/obstacle_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
+    <let name="validator/input/obstacle_pointcloud" value="$(var input_pointcloud)" unless="$(var use_pointcloud_map)"/>
+    <include file="$(find-pkg-share detected_object_validation)/launch/obstacle_pointcloud_based_validator.launch.xml">
+      <arg name="input/detected_objects" value="$(var lidar_detection_model)/objects"/>
+      <arg name="input/obstacle_pointcloud" value="$(var validator/input/obstacle_pointcloud)"/>
+      <arg name="output/objects" value="$(var lidar_detection_model)/validation/objects"/>
     </include>
   </group>
 
   <!-- Merger -->
   <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
-    <arg name="input/object0" value="$(var lidar_detection_model)/objects"/>
+    <arg name="input/object0" value="$(var lidar_detection_model)/validation/objects"/>
     <arg name="input/object1" value="clustering/camera_lidar_fusion/short_range_objects"/>
     <arg name="output/object" value="camera_lidar_fusion/objects"/>
   </include>

diff --git a/perception_launch/launch/object_recognition/detection/detection.launch.xml b/perception_launch/launch/object_recognition/detection/detection.launch.xml
@@ -21,7 +21,7 @@
   <arg name="image_raw7" default=""/>
   <arg name="camera_info7" default=""/>
   <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
-  <arg name="use_vector_map" default="true" description="use vector map in prediction"/>
+  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
 
   <!-- camera lidar fusion based detection-->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_fusion&quot;')">
@@ -43,15 +43,15 @@
       <arg name="image_raw7" value="$(var image_raw7)"/>
       <arg name="camera_info7" value="$(var camera_info7)"/>
       <arg name="image_number" value="$(var image_number)"/>
-      <arg name="use_vector_map" value="$(var use_vector_map)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
   <!-- lidar based detection-->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;lidar&quot;')">
     <include file="$(find-pkg-share perception_launch)/launch/object_recognition/detection/lidar_based_detection.launch.xml">
-      <arg name="use_vector_map" value="$(var use_vector_map)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
   <!-- camera based detection-->

diff --git a/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -1,12 +1,26 @@
 <?xml version="1.0"?>
 <launch>
-  <arg name="use_vector_map" default="true" description="use vector map in prediction"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
+  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+  <arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+
+  <!-- Pointcloud map filter -->
+  <group>
+    <include file="$(find-pkg-share compare_map_segmentation)/launch/voxel_based_compare_map_filter.launch.xml" if="$(var use_pointcloud_map)">
+      <arg name="input" value="$(var input_pointcloud)" />
+      <arg name="input_map" value="/map/pointcloud_map" />
+      <arg name="output" value="pointcloud_map_filtered/pointcloud" />
+      <arg name="distance_threshold" value="0.5" />
+    </include>
+  </group>
 
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
+    <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
+    <let name="clustering/input/pointcloud" value="$(var input_pointcloud)" unless="$(var use_pointcloud_map)"/>
     <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
+      <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
       <arg name="output_clusters" value="clusters"/>
       <arg name="use_pointcloud_map" value="false"/>
     </include>
@@ -44,12 +58,24 @@
     </group>
   </group>
 
+  <!-- Validator -->
+  <group>
+    <let name="validator/input/obstacle_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
+    <let name="validator/input/obstacle_pointcloud" value="$(var input_pointcloud)" unless="$(var use_pointcloud_map)"/>
+    <include file="$(find-pkg-share detected_object_validation)/launch/obstacle_pointcloud_based_validator.launch.xml">
+      <arg name="input/detected_objects" value="$(var lidar_detection_model)/objects"/>
+      <arg name="input/obstacle_pointcloud" value="$(var validator/input/obstacle_pointcloud)"/>
+      <arg name="output/objects" value="$(var lidar_detection_model)/validation/objects"/>
+    </include>
+  </group>
+
   <!-- Merger -->
   <group>
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
-      <arg name="input/object0" value="$(var lidar_detection_model)/objects"/>
+      <arg name="input/object0" value="$(var lidar_detection_model)/validation/objects"/>
       <arg name="input/object1" value="detection_by_tracker/objects"/>
       <arg name="output/object" value="objects"/>
     </include>
   </group>
+
 </launch>
diff --git a/perception_launch/launch/perception.launch.xml b/perception_launch/launch/perception.launch.xml
@@ -21,6 +21,7 @@
   <arg name="camera_info7" default="/sensing/camera/camera7/camera_info"/>
   <arg name="image_number" default="6" description="choose image raw number(0-7)"/>
   <arg name="use_vector_map" default="true" description="use vector map in prediction"/>
+  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_empty_dynamic_object_publisher" default="false" description="if use_empty_dynamic_object_publisher:=true, /perception/object_recognition/objects topic has an empty DynamicObjectArray"/>
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
@@ -80,7 +81,7 @@
           <arg name="image_raw7" value="$(var image_raw7)"/>
           <arg name="camera_info7" value="$(var camera_info7)"/>
           <arg name="image_number" value="$(var image_number)"/>
-          <arg name="use_vector_map" value="false"/>
+          <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
         </include>
       </group>
       <!-- tracking module -->