refactor(detected_object_validation): add an option for filtering and…

… validation (autowarefoundation#4402) * init commit Signed-off-by: ismetatabay <[email protected]> * update occupancy_grid_map path Signed-off-by: ismetatabay <[email protected]> * update argument names Signed-off-by: ismetatabay <[email protected]> * correct radar launch objects_filter_method name Signed-off-by: ismetatabay <[email protected]> * remove radar option Signed-off-by: ismetatabay <[email protected]> --------- Signed-off-by: ismetatabay <[email protected]>
leo-drive · Aug 16, 2023 · d34e8d6 · d34e8d6
1 parent 33055b5
commit d34e8d6
Show file tree

Hide file tree

Showing 7 changed files with 51 additions and 9 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
@@ -225,7 +225,7 @@
   </group>
 
   <!-- Validator -->
-  <group>
+  <group if="$(eval &quot;'$(var objects_validation_method)'=='obstacle_pointcloud'&quot;)">
     <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/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
     <include file="$(find-pkg-share detected_object_validation)/launch/obstacle_pointcloud_based_validator.launch.xml" if="$(var use_validator)">
@@ -235,6 +235,13 @@
     </include>
   </group>
 
+  <group if="$(eval &quot;'$(var objects_validation_method)'=='occupancy_grid'&quot;)">
+    <include file="$(find-pkg-share detected_object_validation)/launch/occupancy_grid_based_validator.launch.xml" if="$(var use_validator)">
+      <arg name="input/detected_objects" value="$(var lidar_detection_model)/objects"/>
+      <arg name="output/objects" value="$(var lidar_detection_model)/validation/objects"/>
+    </include>
+  </group>
+
   <!-- Merger -->
   <group>
     <let name="merger/input/objects" value="$(var lidar_detection_model)/validation/objects" if="$(var use_validator)"/>
@@ -269,4 +276,12 @@
       <arg name="filtering_range_param" value="$(var object_recognition_detection_object_position_filter_param_path)"/>
     </include>
   </group>
+
+  <group if="$(eval &quot;'$(var objects_filter_method)'=='position_filter'&quot;)">
+    <include file="$(find-pkg-share detected_object_validation)/launch/object_position_filter.launch.xml" if="$(var use_object_filter)">
+      <arg name="input/object" value="objects_before_filter"/>
+      <arg name="output/object" value="$(var output/objects)"/>
+      <arg name="filtering_range_param" value="$(var object_recognition_detection_object_position_filter_param_path)"/>
+    </include>
+  </group>
 </launch>
diff --git a/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -105,7 +105,7 @@
   </group>
 
   <!-- Validator -->
-  <group>
+  <group if="$(eval &quot;'$(var objects_validation_method)'=='obstacle_pointcloud'&quot;)">
     <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/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
     <include file="$(find-pkg-share detected_object_validation)/launch/obstacle_pointcloud_based_validator.launch.xml" if="$(var use_validator)">
@@ -115,6 +115,13 @@
     </include>
   </group>
 
+  <group if="$(eval &quot;'$(var objects_validation_method)'=='occupancy_grid'&quot;)">
+    <include file="$(find-pkg-share detected_object_validation)/launch/occupancy_grid_based_validator.launch.xml" if="$(var use_validator)">
+      <arg name="input/detected_objects" value="$(var lidar_detection_model)/objects"/>
+      <arg name="output/objects" value="$(var lidar_detection_model)/validation/objects"/>
+    </include>
+  </group>
+
   <!-- Merger -->
   <group>
     <let name="merger/input/objects" value="$(var lidar_detection_model)/validation/objects" if="$(var use_validator)"/>
@@ -141,11 +148,19 @@
   </group>
 
   <!-- Filter -->
-  <group>
+  <group if="$(eval &quot;'$(var objects_filter_method)'=='lanelet_filter'&quot;)">
     <include file="$(find-pkg-share detected_object_validation)/launch/object_lanelet_filter.launch.xml" if="$(var use_object_filter)">
       <arg name="input/object" value="objects_before_filter"/>
       <arg name="output/object" value="$(var output/objects)"/>
       <arg name="filtering_range_param" value="$(var object_recognition_detection_object_lanelet_filter_param_path)"/>
     </include>
   </group>
+
+  <group if="$(eval &quot;'$(var objects_filter_method)'=='position_filter'&quot;)">
+    <include file="$(find-pkg-share detected_object_validation)/launch/object_position_filter.launch.xml" if="$(var use_object_filter)">
+      <arg name="input/object" value="objects_before_filter"/>
+      <arg name="output/object" value="$(var output/objects)"/>
+      <arg name="filtering_range_param" value="$(var object_recognition_detection_object_position_filter_param_path)"/>
+    </include>
+  </group>
 </launch>
diff --git a/...4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/...4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml
@@ -12,6 +12,8 @@
   <arg name="occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater" description="options: binary_bayes_filter"/>
   <arg name="occupancy_grid_map_updater_param_path"/>
+  <arg name="input_obstacle_pointcloud" default="false" description="subscribe obstacle pointcloud"/>
+  <arg name="input_obstacle_and_raw_pointcloud" default="true" description="subscribe both obstacle and raw pointcloud"/>
 
   <!--pointcloud based method-->
   <group if="$(eval &quot;'$(var occupancy_grid_map_method)'=='pointcloud_based_occupancy_grid_map'&quot;)">
@@ -42,6 +44,8 @@
       <arg name="param_file" value="$(var occupancy_grid_map_param_path)"/>
       <arg name="updater_type" value="$(var occupancy_grid_map_updater)"/>
       <arg name="updater_param_file" value="$(var occupancy_grid_map_updater_param_path)"/>
+      <arg name="input_obstacle_pointcloud" value="$(var input_obstacle_pointcloud)"/>
+      <arg name="input_obstacle_and_raw_pointcloud" value="$(var input_obstacle_and_raw_pointcloud)"/>
     </include>
   </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -68,6 +68,8 @@
   />
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="objects_filter_method" default="lanelet_filter"/>
+  <arg name="objects_validation_method" default="obstacle_pointcloud"/>
   <!-- Traffic Light Recognition Parameters -->
   <arg name="use_traffic_light_recognition" default="true"/>
   <arg name="traffic_light_recognition/enable_fine_detection" default="true"/>

diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt
@@ -80,6 +80,11 @@ rclcpp_components_register_node(object_position_filter
   EXECUTABLE object_position_filter_node
 )
 
+rclcpp_components_register_node(occupancy_grid_based_validator
+  PLUGIN "occupancy_grid_based_validator::OccupancyGridBasedValidator"
+  EXECUTABLE occupancy_grid_based_validator_node
+)
+
 ament_auto_package(INSTALL_TO_SHARE
   launch
   config

diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml
@@ -5,24 +5,23 @@
 
   <arg name="input/laserscan" default="/perception/occupancy_grid_map/virtual_scan/laserscan"/>
   <arg name="input/occupancy_grid_map" default="/perception/object_recognition/detection/validation/occupancy_grid_based/single_frame_occupancy_grid_map"/>
+  <arg name="occupancy_grid_map_param_path" default="$(find-pkg-share probabilistic_occupancy_grid_map)/config/laserscan_based_occupancy_grid_map.param.yaml"/>
+  <arg name="occupancy_grid_map_updater_param_path" default="$(find-pkg-share probabilistic_occupancy_grid_map)/config/binary_bayes_filter_updater.param.yaml"/>
 
   <node pkg="probabilistic_occupancy_grid_map" exec="laserscan_based_occupancy_grid_map_node" name="single_frame_laserscan_occupancy_grid_map" output="screen">
     <remap from="~/input/laserscan" to="$(var input/laserscan)"/>
     <remap from="~/output/occupancy_grid_map" to="$(var input/occupancy_grid_map)"/>
     <param name="input_obstacle_pointcloud" value="false"/>
     <param name="input_obstacle_and_raw_pointcloud" value="false"/>
-    <param name="map_resolution" value="0.2"/>
-    <param name="map_length" value="200.0"/>
-    <param name="map_width" value="20.0"/>
-    <param name="map_frame" value="base_link"/>
-    <param name="enable_single_frame_mode" value="true"/>
+    <param name="updater_type" value="binary_bayes_filter"/>
+    <param from="$(var occupancy_grid_map_param_path)"/>
+    <param from="$(var occupancy_grid_map_updater_param_path)"/>
   </node>
 
   <node pkg="detected_object_validation" exec="occupancy_grid_based_validator_node" name="occupancy_grid_based_validator_node" output="screen">
     <remap from="~/input/detected_objects" to="$(var input/detected_objects)"/>
     <remap from="~/input/occupancy_grid_map" to="$(var input/occupancy_grid_map)"/>
     <remap from="~/output/objects" to="$(var output/objects)"/>
-    <param name="map_frame" value="base_link"/>
     <param name="enable_debug" value="false"/>
   </node>
 </launch>
diff --git a/...tion/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/...tion/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py
@@ -171,6 +171,8 @@ def add_launch_arg(name: str, default_value=None):
                 get_package_share_directory("probabilistic_occupancy_grid_map")
                 + "/config/updater.param.yaml",
             ),
+            add_launch_arg("input_obstacle_pointcloud", "false"),
+            add_launch_arg("input_obstacle_and_raw_pointcloud", "true"),
             add_launch_arg("use_pointcloud_container", "false"),
             add_launch_arg("container_name", "occupancy_grid_map_container"),
             set_container_executable,