Merge pull request #842 from tier4/beta-to-tier4-main-sync

chore: sync beta branch beta/v0.12.0 with tier4/main

common/motion_utils/src/resample/resample.cpp

@@ -109,13 +109,16 @@ std::vector<geometry_msgs::msg::Point> resamplePointVector(
 }
 
 std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
-  const std::vector<geometry_msgs::msg::Pose> & points,
+  const std::vector<geometry_msgs::msg::Pose> & points_raw,
   const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
   const bool use_lerp_for_z)
 {
+  // Remove overlap points for resampling
+  const auto points = motion_utils::removeOverlapPoints(points_raw);
+
   // validate arguments
   if (!resample_utils::validate_arguments(points, resampled_arclength)) {
-    return points;
+    return points_raw;
   }
 
   std::vector<geometry_msgs::msg::Point> position(points.size());

common/object_recognition_utils/include/object_recognition_utils/transform.hpp

@@ -15,15 +15,22 @@
 #ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_
 #define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_
 
+#include <pcl_ros/transforms.hpp>
+
 #include <geometry_msgs/msg/transform.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/point_field.hpp>
 
 #include <boost/optional.hpp>
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #ifdef ROS_DISTRO_GALACTIC
+#include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else
+#include <tf2_eigen/tf2_eigen.hpp>
+
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
@@ -45,6 +52,23 @@ namespace detail
     return boost::none;
   }
 }
+
+[[maybe_unused]] inline boost::optional<Eigen::Matrix4f> getTransformMatrix(
+  const std::string & in_target_frame, const std_msgs::msg::Header & in_cloud_header,
+  const tf2_ros::Buffer & tf_buffer)
+{
+  try {
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    transform_stamped = tf_buffer.lookupTransform(
+      in_target_frame, in_cloud_header.frame_id, in_cloud_header.stamp,
+      rclcpp::Duration::from_seconds(1.0));
+    Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
+    return mat;
+  } catch (tf2::TransformException & e) {
+    RCLCPP_WARN_STREAM(rclcpp::get_logger("detail::getTransformMatrix"), e.what());
+    return boost::none;
+  }
+}
 }  // namespace detail
 
 namespace object_recognition_utils
@@ -79,6 +103,46 @@ bool transformObjects(
   }
   return true;
 }
+template <class T>
+bool transformObjectsWithFeature(
+  const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
+  T & output_msg)
+{
+  output_msg = input_msg;
+  if (input_msg.header.frame_id != target_frame_id) {
+    output_msg.header.frame_id = target_frame_id;
+    tf2::Transform tf_target2objects_world;
+    tf2::Transform tf_target2objects;
+    tf2::Transform tf_objects_world2objects;
+    const auto ros_target2objects_world = detail::getTransform(
+      tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
+    if (!ros_target2objects_world) {
+      return false;
+    }
+    const auto tf_matrix = detail::getTransformMatrix(target_frame_id, input_msg.header, tf_buffer);
+    if (!tf_matrix) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("object_recognition_utils:"), "failed to get transformed matrix");
+      return false;
+    }
+    for (auto & feature_object : output_msg.feature_objects) {
+      // transform object
+      tf2::fromMsg(
+        feature_object.object.kinematics.pose_with_covariance.pose, tf_objects_world2objects);
+      tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
+      tf2::toMsg(tf_target2objects, feature_object.object.kinematics.pose_with_covariance.pose);
+
+      // transform cluster
+      sensor_msgs::msg::PointCloud2 transformed_cluster;
+      pcl_ros::transformPointCloud(*tf_matrix, feature_object.feature.cluster, transformed_cluster);
+      transformed_cluster.header.frame_id = target_frame_id;
+      feature_object.feature.cluster = transformed_cluster;
+    }
+    output_msg.header.frame_id = target_frame_id;
+    return true;
+  }
+  return true;
+}
 }  // namespace object_recognition_utils
 
 #endif  // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_

common/object_recognition_utils/package.xml

@@ -17,7 +17,13 @@
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
   <depend>libboost-dev</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
   <depend>tier4_autoware_utils</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

@@ -79,12 +79,54 @@
       <let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/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="output_clusters" value="euclidean_cluster/clusters"/>
         <arg name="use_pointcloud_map" value="false"/>
         <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
       </include>
     </group>
 
+    <!-- roi based clustering -->
+    <group>
+      <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml">
+        <arg name="input/camera_info0" value="$(var camera_info0)"/>
+        <arg name="input/rois0" value="$(var detection_rois0)"/>
+        <arg name="input/camera_info1" value="$(var camera_info1)"/>
+        <arg name="input/rois1" value="$(var detection_rois1)"/>
+        <arg name="input/camera_info2" value="$(var camera_info2)"/>
+        <arg name="input/rois2" value="$(var detection_rois2)"/>
+        <arg name="input/camera_info3" value="$(var camera_info3)"/>
+        <arg name="input/rois3" value="$(var detection_rois3)"/>
+        <arg name="input/camera_info4" value="$(var camera_info4)"/>
+        <arg name="input/rois4" value="$(var detection_rois4)"/>
+        <arg name="input/camera_info5" value="$(var camera_info5)"/>
+        <arg name="input/rois5" value="$(var detection_rois5)"/>
+        <arg name="input/camera_info6" value="$(var camera_info6)"/>
+        <arg name="input/rois6" value="$(var detection_rois6)"/>
+        <arg name="input/camera_info7" value="$(var camera_info7)"/>
+        <arg name="input/rois7" value="$(var detection_rois7)"/>
+        <arg name="input/rois_number" value="$(var image_number)"/>
+        <arg name="input/image0" value="$(var image_raw0)"/>
+        <arg name="input/image1" value="$(var image_raw1)"/>
+        <arg name="input/image2" value="$(var image_raw2)"/>
+        <arg name="input/image3" value="$(var image_raw3)"/>
+        <arg name="input/image4" value="$(var image_raw4)"/>
+        <arg name="input/image5" value="$(var image_raw5)"/>
+        <arg name="input/image6" value="$(var image_raw6)"/>
+        <arg name="input/image7" value="$(var image_raw7)"/>
+        <arg name="input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+        <arg name="output_clusters" value="roi_cluster/clusters"/>
+      </include>
+    </group>
+
+    <!-- simple_cluster_merger -->
+    <group>
+      <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml">
+        <arg name="input/cluster0" value="euclidean_cluster/clusters"/>
+        <arg name="input/cluster1" value="roi_cluster/clusters"/>
+        <arg name="output/clusters" value="clusters"/>
+      </include>
+    </group>
+
     <group>
       <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
         <arg name="input/objects" value="clusters"/>
@@ -96,6 +138,8 @@
       <let name="input/clustering" value="/perception/object_recognition/detection/clustering/clusters"/>
       <push-ros-namespace namespace="camera_lidar_fusion"/>
       <!-- Fusion camera-lidar to classify -->
+
+      <!-- euclidean clustering -->
       <group>
         <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_cluster_fusion.launch.xml">
           <arg name="input/camera_info0" value="$(var camera_info0)"/>

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml

@@ -143,12 +143,54 @@
           <arg name="input/image5" value="$(var image_raw5)"/>
           <arg name="input/image6" value="$(var image_raw6)"/>
           <arg name="input/image7" value="$(var image_raw7)"/>
-          <arg name="output/clusters" value="clusters"/>
+          <arg name="output/clusters" value="euclidean_cluster/clusters"/>
           <arg name="remove_unknown" value="$(var remove_unknown)"/>
           <arg name="trust_distance" value="$(var trust_distance)"/>
         </include>
       </group>
 
+      <!-- roi based clustering -->
+      <group>
+        <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml">
+          <arg name="input/camera_info0" value="$(var camera_info0)"/>
+          <arg name="input/rois0" value="$(var detection_rois0)"/>
+          <arg name="input/camera_info1" value="$(var camera_info1)"/>
+          <arg name="input/rois1" value="$(var detection_rois1)"/>
+          <arg name="input/camera_info2" value="$(var camera_info2)"/>
+          <arg name="input/rois2" value="$(var detection_rois2)"/>
+          <arg name="input/camera_info3" value="$(var camera_info3)"/>
+          <arg name="input/rois3" value="$(var detection_rois3)"/>
+          <arg name="input/camera_info4" value="$(var camera_info4)"/>
+          <arg name="input/rois4" value="$(var detection_rois4)"/>
+          <arg name="input/camera_info5" value="$(var camera_info5)"/>
+          <arg name="input/rois5" value="$(var detection_rois5)"/>
+          <arg name="input/camera_info6" value="$(var camera_info6)"/>
+          <arg name="input/rois6" value="$(var detection_rois6)"/>
+          <arg name="input/camera_info7" value="$(var camera_info7)"/>
+          <arg name="input/rois7" value="$(var detection_rois7)"/>
+          <arg name="input/rois_number" value="$(var image_number)"/>
+          <arg name="input/image0" value="$(var image_raw0)"/>
+          <arg name="input/image1" value="$(var image_raw1)"/>
+          <arg name="input/image2" value="$(var image_raw2)"/>
+          <arg name="input/image3" value="$(var image_raw3)"/>
+          <arg name="input/image4" value="$(var image_raw4)"/>
+          <arg name="input/image5" value="$(var image_raw5)"/>
+          <arg name="input/image6" value="$(var image_raw6)"/>
+          <arg name="input/image7" value="$(var image_raw7)"/>
+          <arg name="input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+          <arg name="output_clusters" value="roi_cluster/clusters"/>
+        </include>
+      </group>
+
+      <!-- simple_cluster_merger -->
+      <group>
+        <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml">
+          <arg name="input/cluster0" value="euclidean_cluster/clusters"/>
+          <arg name="input/cluster1" value="roi_cluster/clusters"/>
+          <arg name="output/clusters" value="clusters"/>
+        </include>
+      </group>
+
       <group>
         <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
           <arg name="input/objects" value="clusters"/>

launch/tier4_perception_launch/package.xml

@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <exec_depend>cluster_merger</exec_depend>
   <exec_depend>compare_map_segmentation</exec_depend>
   <exec_depend>crosswalk_traffic_light_estimator</exec_depend>
   <exec_depend>detected_object_feature_remover</exec_depend>

perception/cluster_merger/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.8)
+project(cluster_merger)
+
+# find dependencies
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# Targets
+ament_auto_add_library(cluster_merger_node_component SHARED
+  src/cluster_merger/node.cpp
+)
+
+rclcpp_components_register_node(cluster_merger_node_component
+  PLUGIN "cluster_merger::ClusterMergerNode"
+  EXECUTABLE cluster_merger_node)
+
+
+if(BUILD_TESTING)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

perception/cluster_merger/README.md

@@ -0,0 +1,72 @@
+# cluster merger
+
+## Purpose
+
+cluster_merger is a package for merging pointcloud clusters as detected objects with feature type.
+
+## Inner-working / Algorithms
+
+The clusters of merged topics are simply concatenated from clusters of input topics.
+
+## Input / Output
+
+### Input
+
+| Name             | Type                                                     | Description         |
+| ---------------- | -------------------------------------------------------- | ------------------- |
+| `input/cluster0` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters |
+| `input/cluster1` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters |
+
+### Output
+
+| Name              | Type                                                  | Description     |
+| ----------------- | ----------------------------------------------------- | --------------- |
+| `output/clusters` | `autoware_auto_perception_msgs::msg::DetectedObjects` | merged clusters |
+
+## Parameters
+
+| Name              | Type   | Description                          | Default value |
+| :---------------- | :----- | :----------------------------------- | :------------ |
+| `output_frame_id` | string | The header frame_id of output topic. | base_link     |
+
+## Assumptions / Known limits
+
+<!-- Write assumptions and limitations of your implementation.
+
+Example:
+  This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
+  Also, this algorithm doesn't care about blind spots. In general, since too close obstacles aren't visible due to the sensing performance limit, please take enough margin to obstacles.
+-->
+
+## (Optional) Error detection and handling
+
+<!-- Write how to detect errors and how to recover from them.
+
+Example:
+  This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
+-->
+
+## (Optional) Performance characterization
+
+<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary.
+
+Example:
+  ### Complexity
+
+  This algorithm is O(N).
+
+  ### Processing time
+
+  ...
+-->
+
+## (Optional) References/External links
+
+<!-- Write links you referred to when you implemented.
+
+Example:
+  [1] {link_to_a_thesis}
+  [2] {link_to_an_issue}
+-->
+
+## (Optional) Future extensions / Unimplemented parts