fix: integration miss related to camera lidar fusion (autowarefoundat…

…ion#481) * fix integration miss Signed-off-by: Yukihiro Saito <[email protected]> * bug fix Signed-off-by: Yukihiro Saito <[email protected]> * add detection by tracker Signed-off-by: Yukihiro Saito <[email protected]> * Update launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
satoshi-ota · Mar 10, 2022 · 6e14982 · 6e14982
1 parent 5c9302d
commit 6e14982
Show file tree

Hide file tree

Showing 6 changed files with 43 additions and 19 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
@@ -37,6 +37,7 @@
     <push-ros-namespace namespace="euclidean"/>
     <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
       <arg name="output_clusters" value="lidar/clusters"/>
+      <arg name="use_pointcloud_map" value="false"/>
     </include>
     <include file="$(find-pkg-share roi_cluster_fusion)/launch/roi_cluster_fusion.launch.xml">
       <arg name="input/camera_info0" value="$(var camera_info0)"/>
@@ -68,10 +69,8 @@
       <arg name="output" value="camera_lidar_fusion/clusters"/>
     </include>
     <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
-      <arg name="use_map_current" value="$(var use_vector_map)"/>
       <arg name="input/objects" value="camera_lidar_fusion/clusters" />
       <arg name="output/objects" value="camera_lidar_fusion/objects_with_feature" />
-      <arg name="orientation_reliable" value="false"/>
     </include>
     <!-- convert DynamicObjectsWithFeatureArray to DynamicObjects -->
     <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
@@ -86,25 +85,48 @@
     </include>
   </group>
 
+  <!-- CenterPoint -->
   <group if="$(eval &quot;'$(var lidar_detection_model)'=='centerpoint'&quot;)">
     <push-ros-namespace namespace="centerpoint"/>
     <include file="$(find-pkg-share lidar_centerpoint)/launch/lidar_centerpoint.launch.xml">
       <arg name="output/objects" value="objects" />
     </include>
   </group>
 
+  <!-- Lidar Apollo Instance Segmentation -->
   <group if="$(eval &quot;'$(var lidar_detection_model)'=='apollo'&quot;)">
     <push-ros-namespace namespace="apollo"/>
-    <include file="$(find-pkg-share lidar_apollo_instance_segmentation)/launch/lidar_apollo_instance_segmentation.launch.xml" />
-    <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
-      <arg name="output/objects" value="objects"/>
-      <arg name="use_map_current" value="$(var use_vector_map)"/>
+    <group>
+      <include file="$(find-pkg-share lidar_apollo_instance_segmentation)/launch/lidar_apollo_instance_segmentation.launch.xml" />
+      <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
+        <arg name="output/objects" value="objects_with_feature"/>
+        <arg name="use_vehicle_reference_yaw" value="true"/>
+      </include>
+      <!-- convert DynamicObjectsWithFeatureArray to DynamicObjects -->
+      <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="objects" />
+      </include>
+    </group>
+  </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>
 
+  <!-- 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/object1" value="euclidean/camera_lidar_fusion/short_range_objects"/>
+    <arg name="output/object" value="camera_lidar_fusion/objects"/>
+  </include>
+  <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
+    <arg name="input/object0" value="detection_by_tracker/objects"/>
+    <arg name="input/object1" value="camera_lidar_fusion/objects"/>
     <arg name="output/object" value="objects"/>
   </include>
+
 </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
@@ -33,7 +33,7 @@
       <include file="$(find-pkg-share lidar_apollo_instance_segmentation)/launch/lidar_apollo_instance_segmentation.launch.xml" />
       <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
         <arg name="output/objects" value="objects_with_feature"/>
-        <arg name="use_map_current" value="$(var use_vector_map)"/>
+        <arg name="use_vehicle_reference_yaw" value="true"/>
       </include>
       <!-- convert DynamicObjectsWithFeatureArray to DynamicObjects -->
       <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">

diff --git a/perception/roi_cluster_fusion/src/roi_cluster_fusion_nodelet.cpp b/perception/roi_cluster_fusion/src/roi_cluster_fusion_nodelet.cpp
@@ -47,7 +47,7 @@ Debugger::Debugger(rclcpp::Node * node, const int camera_num) : node_(node)
   for (int id = 0; id < camera_num; ++id) {
     auto sub = image_transport::create_subscription(
       node, "input/image_raw" + std::to_string(id),
-      boost::bind(&Debugger::imageCallback, this, _1, id), "raw");
+      boost::bind(&Debugger::imageCallback, this, _1, id), "raw", rmw_qos_profile_sensor_data);
     image_subs_.push_back(sub);
     if (node->has_parameter("format")) {
       node->undeclare_parameter("format");

diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml
@@ -2,15 +2,15 @@
 
   <arg name="input/objects" default="labeled_clusters"/>
   <arg name="output/objects" default="shape_estimated_objects"/>
-  <arg name="use_map_current" default="true"/>
+  <arg name="use_filter" default="true"/>
   <arg name="use_corrector" default="true"/>
   <arg name="node_name" default="shape_estimation"/>
-  <arg name="use_vehicle_reference_yaw" default="true"/>
+  <arg name="use_vehicle_reference_yaw" default="false"/>
   <node pkg="shape_estimation" exec="shape_estimation" name="$(var node_name)" output="screen">
     <remap from="input" to="$(var input/objects)" />
     <remap from="objects" to="$(var output/objects)" />
+    <param name="use_filter" value="$(var use_filter)" />
     <param name="use_corrector" value="$(var use_corrector)" />
-    <param name="use_map_current" value="$(var use_map_current)" />
     <param name="use_vehicle_reference_yaw" value="$(var use_vehicle_reference_yaw)"/>
   </node>
 </launch>
diff --git a/sensing/image_transport_decompressor/CMakeLists.txt b/sensing/image_transport_decompressor/CMakeLists.txt
@@ -39,4 +39,6 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_auto_package()
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)
diff --git a/sensing/image_transport_decompressor/launch/image_transport_decompressor.launch.xml b/sensing/image_transport_decompressor/launch/image_transport_decompressor.launch.xml
@@ -1,9 +1,9 @@
 <launch>
-    <arg name="input_topic_name" default="/input/compressed_image" />
-    <arg name="output_topic_name" default="/output/raw_image" />
+  <arg name="input_topic_name" default="/input/compressed_image" />
+  <arg name="output_topic_name" default="/output/raw_image" />
 
-    <node pkg="image_transport_decompressor" exec="image_transport_decompressor_node" name="image_transport_decompressor_node">
-      <remap from="input" to="$(var input_topic_name)" />
-      <remap from="output" to="$(var output_topic_name)" />
-    </node>
-  </launch>
+  <node pkg="image_transport_decompressor" exec="image_transport_decompressor_node" name="$(anon image_transport_decompressor_node)">
+    <remap from="~/input/compressed_image" to="$(var input_topic_name)" />
+    <remap from="~/output/raw_image" to="$(var output_topic_name)" />
+  </node>
+</launch>