Merge branch 'main' into test/image_projection_based_fusion/add_calc_iou_tests

Author: SamratThapa120

perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp

@@ -32,6 +32,10 @@ class EuclideanCluster : public EuclideanClusterInterface
   bool cluster(
     const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
     std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) override;
+
+  bool cluster(
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud,
+    tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) override;
   void setTolerance(float tolerance) { tolerance_ = tolerance; }
 
 private:

perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp

@@ -16,6 +16,9 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
+
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
@@ -41,6 +44,10 @@ class EuclideanClusterInterface
     const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
     std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) = 0;
 
+  virtual bool cluster(
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_msg,
+    tier4_perception_msgs::msg::DetectedObjectsWithFeature & output_clusters) = 0;
+
 protected:
   bool use_height_ = true;
   int min_cluster_size_;

perception/euclidean_cluster/include/euclidean_cluster/utils.hpp

@@ -31,6 +31,9 @@ void convertPointCloudClusters2Msg(
   const std_msgs::msg::Header & header,
   const std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters,
   tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg);
+void convertPointCloudClusters2Msg(
+  const std_msgs::msg::Header & header, const std::vector<sensor_msgs::msg::PointCloud2> & clusters,
+  tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg);
 void convertObjectMsg2SensorMsg(
   const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input,
   sensor_msgs::msg::PointCloud2 & output);

perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp

@@ -35,6 +35,9 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface
   bool cluster(
     const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
     std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) override;
+  bool cluster(
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud,
+    tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) override;
   void setVoxelLeafSize(float voxel_leaf_size) { voxel_leaf_size_ = voxel_leaf_size; }
   void setTolerance(float tolerance) { tolerance_ = tolerance; }
   void setMinPointsNumberPerVoxel(int min_points_number_per_voxel)

perception/euclidean_cluster/lib/euclidean_cluster.cpp

@@ -33,6 +33,15 @@ EuclideanCluster::EuclideanCluster(
 : EuclideanClusterInterface(use_height, min_cluster_size, max_cluster_size), tolerance_(tolerance)
 {
 }
+// TODO(badai-nguyen): implement input field copy for euclidean_cluster.cpp
+bool EuclideanCluster::cluster(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg,
+  tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters)
+{
+  (void)pointcloud_msg;
+  (void)clusters;
+  return false;
+}
 
 bool EuclideanCluster::cluster(
   const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,

perception/euclidean_cluster/lib/utils.cpp

@@ -62,6 +62,25 @@ void convertPointCloudClusters2Msg(
     msg.feature_objects.push_back(feature_object);
   }
 }
+
+void convertPointCloudClusters2Msg(
+  const std_msgs::msg::Header & header, const std::vector<sensor_msgs::msg::PointCloud2> & clusters,
+  tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg)
+{
+  msg.header = header;
+  for (const auto & ros_pointcloud : clusters) {
+    tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
+    feature_object.feature.cluster = ros_pointcloud;
+    feature_object.object.kinematics.pose_with_covariance.pose.position =
+      getCentroid(ros_pointcloud);
+    autoware_auto_perception_msgs::msg::ObjectClassification classification;
+    classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
+    classification.probability = 1.0f;
+    feature_object.object.classification.emplace_back(classification);
+    msg.feature_objects.push_back(feature_object);
+  }
+}
+
 void convertObjectMsg2SensorMsg(
   const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input,
   sensor_msgs::msg::PointCloud2 & output)

perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp

@@ -40,14 +40,27 @@ VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster(
   min_points_number_per_voxel_(min_points_number_per_voxel)
 {
 }
-
+// TODO(badai-nguyen): remove this function when field copying also implemented for
+// euclidean_cluster.cpp
 bool VoxelGridBasedEuclideanCluster::cluster(
   const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
   std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters)
+{
+  (void)pointcloud;
+  (void)clusters;
+  return false;
+}
+
+bool VoxelGridBasedEuclideanCluster::cluster(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg,
+  tier4_perception_msgs::msg::DetectedObjectsWithFeature & objects)
 {
   // TODO(Saito) implement use_height is false version
 
   // create voxel
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
+  int point_step = pointcloud_msg->point_step;
+  pcl::fromROSMsg(*pointcloud_msg, *pointcloud);
   pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_map_ptr(new pcl::PointCloud<pcl::PointXYZ>);
   voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, 100000.0);
   voxel_grid_.setMinimumPointsNumberPerVoxel(min_points_number_per_voxel_);
@@ -81,36 +94,69 @@ bool VoxelGridBasedEuclideanCluster::cluster(
 
   // create map to search cluster index from voxel grid index
   std::unordered_map</* voxel grid index */ int, /* cluster index */ int> map;
+  std::vector<sensor_msgs::msg::PointCloud2> temporary_clusters;  // no check about cluster size
+  std::vector<size_t> clusters_data_size;
+  temporary_clusters.resize(cluster_indices.size());
   for (size_t cluster_idx = 0; cluster_idx < cluster_indices.size(); ++cluster_idx) {
     const auto & cluster = cluster_indices.at(cluster_idx);
+    auto & temporary_cluster = temporary_clusters.at(cluster_idx);
     for (const auto & point_idx : cluster.indices) {
       map[point_idx] = cluster_idx;
     }
+    temporary_cluster.height = pointcloud_msg->height;
+    temporary_cluster.fields = pointcloud_msg->fields;
+    temporary_cluster.point_step = point_step;
+    temporary_cluster.data.resize(max_cluster_size_ * point_step);
+    clusters_data_size.push_back(0);
   }
 
   // create vector of point cloud cluster. vector index is voxel grid index.
-  std::vector<pcl::PointCloud<pcl::PointXYZ>> temporary_clusters;  // no check about cluster size
-  temporary_clusters.resize(cluster_indices.size());
-  for (const auto & point : pointcloud->points) {
+  for (size_t i = 0; i < pointcloud->points.size(); ++i) {
+    const auto & point = pointcloud->points.at(i);
     const int index =
       voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z));
     if (map.find(index) != map.end()) {
-      temporary_clusters.at(map[index]).points.push_back(point);
+      auto & cluster_data_size = clusters_data_size.at(map[index]);
+      if (cluster_data_size + point_step > std::size_t(max_cluster_size_ * point_step)) {
+        continue;
+      }
+      std::memcpy(
+        &temporary_clusters.at(map[index]).data[cluster_data_size],
+        &pointcloud_msg->data[i * point_step], point_step);
+      cluster_data_size += point_step;
     }
   }
 
   // build output and check cluster size
   {
-    for (const auto & cluster : temporary_clusters) {
-      if (!(min_cluster_size_ <= static_cast<int>(cluster.points.size()) &&
-            static_cast<int>(cluster.points.size()) <= max_cluster_size_)) {
+    for (size_t i = 0; i < temporary_clusters.size(); ++i) {
+      auto & i_cluster_data_size = clusters_data_size.at(i);
+      if (!(min_cluster_size_ <= static_cast<int>(i_cluster_data_size / point_step) &&
+            static_cast<int>(i_cluster_data_size / point_step) <= max_cluster_size_)) {
         continue;
       }
-      clusters.push_back(cluster);
-      clusters.back().width = cluster.points.size();
-      clusters.back().height = 1;
-      clusters.back().is_dense = false;
+      const auto & cluster = temporary_clusters.at(i);
+      tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
+      feature_object.feature.cluster = cluster;
+      feature_object.feature.cluster.data.resize(i_cluster_data_size);
+      feature_object.feature.cluster.header = pointcloud_msg->header;
+      feature_object.feature.cluster.is_bigendian = pointcloud_msg->is_bigendian;
+      feature_object.feature.cluster.is_dense = pointcloud_msg->is_dense;
+      feature_object.feature.cluster.point_step = point_step;
+      feature_object.feature.cluster.row_step = i_cluster_data_size / pointcloud_msg->height;
+      feature_object.feature.cluster.width =
+        i_cluster_data_size / point_step / pointcloud_msg->height;
+
+      feature_object.object.kinematics.pose_with_covariance.pose.position =
+        getCentroid(feature_object.feature.cluster);
+      autoware_auto_perception_msgs::msg::ObjectClassification classification;
+      classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
+      classification.probability = 1.0f;
+      feature_object.object.classification.emplace_back(classification);
+
+      objects.feature_objects.push_back(feature_object);
     }
+    objects.header = pointcloud_msg->header;
   }
 
   return true;

perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp

@@ -55,24 +55,15 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud(
   stop_watch_ptr_->toc("processing_time", true);
 
   // convert ros to pcl
-  pcl::PointCloud<pcl::PointXYZ>::Ptr raw_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
   if (input_msg->data.empty()) {
     // NOTE: prevent pcl log spam
     RCLCPP_WARN_STREAM_THROTTLE(
       this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!");
-  } else {
-    pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr);
   }
-
-  // clustering
-  std::vector<pcl::PointCloud<pcl::PointXYZ>> clusters;
-  if (!raw_pointcloud_ptr->empty()) {
-    cluster_->cluster(raw_pointcloud_ptr, clusters);
-  }
-
-  // build output msg
+  // cluster and build output msg
   tier4_perception_msgs::msg::DetectedObjectsWithFeature output;
-  convertPointCloudClusters2Msg(input_msg->header, clusters, output);
+
+  cluster_->cluster(input_msg, output);
   cluster_pub_->publish(output);
 
   // build debug msg

planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp

@@ -87,12 +87,8 @@ boost::optional<LineString2d> NoStoppingAreaModule::getStopLineGeometry2d(
         const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}};
         std::vector<Point2d> collision_points;
         bg::intersection(area_poly, line, collision_points);
-        if (collision_points.empty()) {
-          continue;
-        }
-        const double yaw = tier4_autoware_utils::calcAzimuthAngle(p0, p1);
         if (!collision_points.empty()) {
-          geometry_msgs::msg::Point left_point;
+          const double yaw = tier4_autoware_utils::calcAzimuthAngle(p0, p1);
           const double w = planner_data_->vehicle_info_.vehicle_width_m;
           const double l = stop_line_margin;
           stop_line.emplace_back(
@@ -306,7 +302,6 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
 
   const int num_ignore_nearest = 1;  // Do not consider nearest lane polygon
   size_t ego_area_start_idx = closest_idx + num_ignore_nearest;
-  size_t ego_area_end_idx = ego_area_start_idx;
   // return if area size is not intentional
   if (no_stopping_area_reg_elem_.noStoppingAreas().size() != 1) {
     return ego_area;
@@ -330,7 +325,7 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
   }
   double dist_from_area_sum = 0.0;
   // decide end idx with extract distance
-  ego_area_end_idx = ego_area_start_idx;
+  size_t ego_area_end_idx = ego_area_start_idx;
   for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) {
     dist_from_start_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1));
     const auto & p = pp.at(i).point.pose.position;

planning/behavior_velocity_planner/src/node.cpp

@@ -311,6 +311,7 @@ void BehaviorVelocityPlannerNode::onParam()
   // lock(mutex_);
   planner_data_.velocity_smoother_ =
     std::make_unique<motion_velocity_smoother::AnalyticalJerkConstrainedSmoother>(*this);
+  planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m);
 }
 
 void BehaviorVelocityPlannerNode::onLaneletMap(

planning/rtc_interface/include/rtc_interface/rtc_interface.hpp

@@ -84,6 +84,7 @@ class RTCInterface
   rclcpp::Service<AutoMode>::SharedPtr srv_auto_mode_;
   rclcpp::CallbackGroup::SharedPtr callback_group_;
   rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Clock::SharedPtr clock_;
   rclcpp::Logger logger_;
 
   Module module_;

planning/rtc_interface/src/rtc_interface.cpp

@@ -81,7 +81,8 @@ Module getModuleType(const std::string & module_name)
 namespace rtc_interface
 {
 RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc)
-: logger_{node->get_logger().get_child("RTCInterface[" + name + "]")},
+: clock_{node->get_clock()},
+  logger_{node->get_logger().get_child("RTCInterface[" + name + "]")},
   is_auto_mode_enabled_{!enable_rtc},
   is_locked_{false}
 {
@@ -270,9 +271,7 @@ void RTCInterface::removeExpiredCooperateStatus()
   std::lock_guard<std::mutex> lock(mutex_);
   const auto itr = std::remove_if(
     registered_status_.statuses.begin(), registered_status_.statuses.end(),
-    [](const auto & status) {
-      return (rclcpp::Clock{RCL_ROS_TIME}.now() - status.stamp).seconds() > 10.0;
-    });
+    [this](const auto & status) { return (clock_->now() - status.stamp).seconds() > 10.0; });
 
   registered_status_.statuses.erase(itr, registered_status_.statuses.end());
 }