Merge branch 'main' into feat/update-tlr-util-function

Author: Shin-kyoto

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_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -26,6 +26,7 @@
 #include <rclcpp/node.hpp>
 #include <rclcpp/time.hpp>
 
+#include <limits>
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -138,9 +139,15 @@ class AvoidanceModule : public SceneModuleInterface
   void removeCandidateRTCStatus()
   {
     if (rtc_interface_ptr_map_.at("left")->isRegistered(candidate_uuid_)) {
-      rtc_interface_ptr_map_.at("left")->removeCooperateStatus(candidate_uuid_);
-    } else if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) {
-      rtc_interface_ptr_map_.at("right")->removeCooperateStatus(candidate_uuid_);
+      rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
+        candidate_uuid_, true, State::FAILED, std::numeric_limits<double>::lowest(),
+        std::numeric_limits<double>::lowest(), clock_->now());
+    }
+
+    if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) {
+      rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
+        candidate_uuid_, true, State::FAILED, std::numeric_limits<double>::lowest(),
+        std::numeric_limits<double>::lowest(), clock_->now());
     }
   }
 
@@ -362,10 +369,21 @@ class AvoidanceModule : public SceneModuleInterface
 
     unlockNewModuleLaunch();
 
+    for (const auto & left_shift : left_shift_array_) {
+      rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
+        left_shift.uuid, true, State::FAILED, std::numeric_limits<double>::lowest(),
+        std::numeric_limits<double>::lowest(), clock_->now());
+    }
+
+    for (const auto & right_shift : right_shift_array_) {
+      rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
+        right_shift.uuid, true, State::FAILED, std::numeric_limits<double>::lowest(),
+        std::numeric_limits<double>::lowest(), clock_->now());
+    }
+
     if (!path_shifter_.getShiftLines().empty()) {
       left_shift_array_.clear();
       right_shift_array_.clear();
-      removeRTCStatus();
     }
 
     generator_.reset();

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -1283,7 +1283,6 @@ void AvoidanceModule::updateData()
 void AvoidanceModule::processOnEntry()
 {
   initVariables();
-  removeRTCStatus();
 }
 
 void AvoidanceModule::processOnExit()

planning/behavior_velocity_planner/README.md

@@ -67,4 +67,4 @@ The handling of traffic light information varies depending on the usage. In the
 | traffic_light(sim, `is_simulation = true`) <ul> <li>`info` is current information</li></ul>                | GO                       | traffic_light uses the perceived traffic light information at present directly. <ul><li>If `info` is timeout, STOP whatever the color is</li> <li>If `info` is not timeout, then act according to the color. If `info` is `UNKNOWN`, STOP</li></ul> {: rowspan=2}                    |
 | traffic_light(real, `is_simulation = false`) <ul> <li>`info` is current information</li></ul>              | STOP                     | &#8288 {: style="padding:0"}                                                                                                                                                                                                                                                         |
 | crosswalk with Traffic Light(`is_simulation = *`) <ul> <li>`info` is current information</li></ul>         | default                  | <ul> <li>If `disable_yield_for_new_stopped_object` is true, each sub scene_module ignore newly detected pedestrians after module instantiation.</li> <li>If `ignore_with_traffic_light` is true, occlusion detection is skipped.</li></ul>                                           |
-| map_based_prediction(`is_simulation = *`) <ul> <li>`info` is current information</li></ul>                 | default                  | If a pedestrian traffic light is<ul> <li>RED, surrouding pedestrians are not predicted.</li> <li>GREEN, stopped pedestrians are not predicted.</li></ul>                                                                                                                             |
+| map_based_prediction(`is_simulation = *`) <ul> <li>`info` is current information</li></ul>                 | default                  | If a pedestrian traffic light is<ul> <li>RED, surrounding pedestrians are not predicted.</li> <li>GREEN, stopped pedestrians are not predicted.</li></ul>                                                                                                                            |

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/planning_test_utils/test/test_mock_data_parser.cpp

@@ -80,11 +80,12 @@ TEST(ParseFunctions, CompleteYAMLTest)
 
   // Test parsing of segments
   std::vector<LaneletSegment> segments = parse_segments(config["segments"]);
-  ASSERT_EQ(segments.size(), 1);  // Assuming only one segment in the provided YAML for this test
+  ASSERT_EQ(
+    segments.size(), uint64_t(1));  // Assuming only one segment in the provided YAML for this test
 
   const auto & segment0 = segments[0];
   EXPECT_EQ(segment0.preferred_primitive.id, 11);
-  EXPECT_EQ(segment0.primitives.size(), 2);
+  EXPECT_EQ(segment0.primitives.size(), uint64_t(2));
   EXPECT_EQ(segment0.primitives[0].id, 22);
   EXPECT_EQ(segment0.primitives[0].primitive_type, "lane");
   EXPECT_EQ(segment0.primitives[1].id, 33);
@@ -118,10 +119,10 @@ TEST(ParseFunction, CompleteFromFilename)
 
   ASSERT_EQ(
     lanelet_route.segments.size(),
-    2);  // Assuming only one segment in the provided YAML for this test
+    uint64_t(2));  // Assuming only one segment in the provided YAML for this test
   const auto & segment1 = lanelet_route.segments[1];
   EXPECT_EQ(segment1.preferred_primitive.id, 44);
-  EXPECT_EQ(segment1.primitives.size(), 4);
+  EXPECT_EQ(segment1.primitives.size(), uint64_t(4));
   EXPECT_EQ(segment1.primitives[0].id, 55);
   EXPECT_EQ(segment1.primitives[0].primitive_type, "lane");
   EXPECT_EQ(segment1.primitives[1].id, 66);

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());
 }