feat(autoware_motion_velocity_obstacle_<stop/slow_down>_module): brings Point Cloud handling to this module (autowarefoundation#10112)

pointcloud handling for slowdown and stop module

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

Author: PanConChicharron

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/config/obstacle_slow_down.param.yaml

@@ -42,6 +42,14 @@
           pedestrian: true
           pointcloud: false
 
+        pointcloud:
+          pointcloud_voxel_grid_x: 0.05
+          pointcloud_voxel_grid_y: 0.05
+          pointcloud_voxel_grid_z: 100000.0
+          pointcloud_cluster_tolerance: 1.0
+          pointcloud_min_cluster_size: 1
+          pointcloud_max_cluster_size: 100000
+
         min_lat_margin: 0.0  # lateral margin between obstacle and trajectory band with ego's width to avoid the conflict with the obstacle_stop
         max_lat_margin: 1.1  # lateral margin between obstacle and trajectory band with ego's width
         lat_hysteresis_margin: 0.2

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp

@@ -231,6 +231,94 @@ void ObstacleSlowDownModule::update_parameters(
 {
 }
 
+std::vector<autoware::motion_velocity_planner::SlowDownPointData>
+ObstacleSlowDownModule::convert_point_cloud_to_slow_down_points(
+  const PlannerData::Pointcloud & pointcloud, const std::vector<TrajectoryPoint> & traj_points,
+  const VehicleInfo & vehicle_info, const size_t ego_idx)
+{
+  if (pointcloud.pointcloud.empty()) {
+    return {};
+  }
+
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
+  const auto & p = obstacle_filtering_param_;
+
+  std::vector<autoware::motion_velocity_planner::SlowDownPointData> slow_down_points;
+
+  // 1. transform pointcloud
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_ptr =
+    std::make_shared<pcl::PointCloud<pcl::PointXYZ>>(pointcloud.pointcloud);
+  // 2. downsample & cluster pointcloud
+  PointCloud::Ptr filtered_points_ptr(new PointCloud);
+  pcl::VoxelGrid<pcl::PointXYZ> filter;
+  filter.setInputCloud(pointcloud_ptr);
+  filter.setLeafSize(
+    p.pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_x,
+    p.pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_y,
+    p.pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_z);
+  filter.filter(*filtered_points_ptr);
+
+  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+  tree->setInputCloud(filtered_points_ptr);
+  std::vector<pcl::PointIndices> clusters;
+  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+  ec.setClusterTolerance(p.pointcloud_obstacle_filtering_param.pointcloud_cluster_tolerance);
+  ec.setMinClusterSize(p.pointcloud_obstacle_filtering_param.pointcloud_min_cluster_size);
+  ec.setMaxClusterSize(p.pointcloud_obstacle_filtering_param.pointcloud_max_cluster_size);
+  ec.setSearchMethod(tree);
+  ec.setInputCloud(filtered_points_ptr);
+  ec.extract(clusters);
+
+  // 3. convert clusters to obstacles
+  for (const auto & cluster_indices : clusters) {
+    double ego_to_slow_down_front_collision_distance = std::numeric_limits<double>::max();
+    double ego_to_slow_down_back_collision_distance = std::numeric_limits<double>::min();
+    double lat_dist_from_obstacle_to_traj = std::numeric_limits<double>::max();
+    std::optional<geometry_msgs::msg::Point> slow_down_front_collision_point = std::nullopt;
+    std::optional<geometry_msgs::msg::Point> slow_down_back_collision_point = std::nullopt;
+
+    for (const auto & index : cluster_indices.indices) {
+      const auto obstacle_point = autoware::motion_velocity_planner::utils::to_geometry_point(
+        filtered_points_ptr->points[index]);
+      const auto current_lat_dist_from_obstacle_to_traj =
+        autoware::motion_utils::calcLateralOffset(traj_points, obstacle_point);
+      const auto min_lat_dist_to_traj_poly =
+        std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info.vehicle_width_m;
+
+      if (min_lat_dist_to_traj_poly >= p.max_lat_margin) {
+        continue;
+      }
+
+      const auto current_ego_to_obstacle_distance =
+        autoware::motion_velocity_planner::utils::calc_distance_to_front_object(
+          traj_points, ego_idx, obstacle_point);
+      if (!current_ego_to_obstacle_distance) {
+        continue;
+      }
+
+      lat_dist_from_obstacle_to_traj =
+        std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj);
+
+      if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) {
+        slow_down_front_collision_point = obstacle_point;
+        ego_to_slow_down_front_collision_distance = *current_ego_to_obstacle_distance;
+      } else if (*current_ego_to_obstacle_distance > ego_to_slow_down_back_collision_distance) {
+        slow_down_back_collision_point = obstacle_point;
+        ego_to_slow_down_back_collision_distance = *current_ego_to_obstacle_distance;
+      }
+    }
+
+    if (slow_down_front_collision_point) {
+      slow_down_points.emplace_back(
+        slow_down_front_collision_point, slow_down_back_collision_point,
+        lat_dist_from_obstacle_to_traj);
+    }
+  }
+
+  return slow_down_points;
+}
+
 VelocityPlanningResult ObstacleSlowDownModule::plan(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & raw_trajectory_points,
   [[maybe_unused]] const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &
@@ -249,12 +337,22 @@ VelocityPlanningResult ObstacleSlowDownModule::plan(
     planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold,
     planner_data->trajectory_polygon_collision_check.decimate_trajectory_step_length, 0.0);
 
-  const auto slow_down_obstacles = filter_slow_down_obstacle_for_predicted_object(
+  auto slow_down_obstacles_for_predicted_object = filter_slow_down_obstacle_for_predicted_object(
     planner_data->current_odometry, planner_data->ego_nearest_dist_threshold,
     planner_data->ego_nearest_yaw_threshold, raw_trajectory_points, decimated_traj_points,
     planner_data->objects, rclcpp::Time(planner_data->predicted_objects_header.stamp),
     planner_data->vehicle_info_, planner_data->trajectory_polygon_collision_check);
 
+  auto slow_down_obstacles_for_point_cloud = filter_slow_down_obstacle_for_point_cloud(
+    planner_data->current_odometry, raw_trajectory_points, decimated_traj_points,
+    planner_data->no_ground_pointcloud, planner_data->vehicle_info_,
+    planner_data->trajectory_polygon_collision_check,
+    planner_data->find_index(raw_trajectory_points, planner_data->current_odometry.pose.pose));
+
+  const auto slow_down_obstacles = autoware::motion_velocity_planner::utils::concat_vectors(
+    std::move(slow_down_obstacles_for_predicted_object),
+    std::move(slow_down_obstacles_for_point_cloud));
+
   VelocityPlanningResult result;
   result.slowdown_intervals = plan_slow_down(
     planner_data, raw_trajectory_points, slow_down_obstacles, result.velocity_limit,
@@ -348,6 +446,53 @@ ObstacleSlowDownModule::filter_slow_down_obstacle_for_predicted_object(
   return slow_down_obstacles;
 }
 
+std::vector<SlowDownObstacle> ObstacleSlowDownModule::filter_slow_down_obstacle_for_point_cloud(
+  const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+  const std::vector<TrajectoryPoint> & decimated_traj_points,
+  const PlannerData::Pointcloud & point_cloud, const VehicleInfo & vehicle_info,
+  const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check, size_t ego_idx)
+{
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
+  // calculate collision points with trajectory with lateral stop margin
+  // NOTE: For additional margin, hysteresis is not divided by two.
+  const auto & p = obstacle_filtering_param_;
+  const auto & tp = trajectory_polygon_collision_check;
+  const auto decimated_traj_polys_with_lat_margin = polygon_utils::create_one_step_polygons(
+    decimated_traj_points, vehicle_info, odometry.pose.pose,
+    p.max_lat_margin + p.lat_hysteresis_margin, tp.enable_to_consider_current_pose,
+    tp.time_to_convergence, tp.decimate_trajectory_step_length);
+  debug_data_ptr_->decimated_traj_polys = decimated_traj_polys_with_lat_margin;
+
+  // Get Objects
+  const std::vector<autoware::motion_velocity_planner::SlowDownPointData> slow_down_points_data =
+    convert_point_cloud_to_slow_down_points(point_cloud, traj_points, vehicle_info, ego_idx);
+
+  // slow down
+  std::vector<SlowDownObstacle> slow_down_obstacles;
+  for (const auto & slow_down_point_data : slow_down_points_data) {
+    if (!slow_down_point_data.front) {
+      continue;
+    }
+    const auto & front_collision_point = *slow_down_point_data.front;
+    const auto & back_collision_point = slow_down_point_data.back.value_or(front_collision_point);
+
+    const auto slow_down_obstacle = create_slow_down_obstacle_for_point_cloud(
+      rclcpp::Time(point_cloud.pointcloud.header.stamp), front_collision_point,
+      back_collision_point, slow_down_point_data.lat_dist_to_traj);
+
+    if (slow_down_obstacle) {
+      slow_down_obstacles.push_back(*slow_down_obstacle);
+      continue;
+    }
+  }
+
+  RCLCPP_DEBUG(
+    logger_, "The number of output obstacles of filter_slow_down_obstacles is %ld",
+    slow_down_obstacles.size());
+  return slow_down_obstacles;
+}
+
 std::optional<SlowDownObstacle>
 ObstacleSlowDownModule::create_slow_down_obstacle_for_predicted_object(
   const std::vector<TrajectoryPoint> & traj_points,
@@ -485,6 +630,37 @@ ObstacleSlowDownModule::create_slow_down_obstacle_for_predicted_object(
     back_collision_point};
 }
 
+std::optional<SlowDownObstacle> ObstacleSlowDownModule::create_slow_down_obstacle_for_point_cloud(
+  const rclcpp::Time & stamp, const geometry_msgs::msg::Point & front_collision_point,
+  const geometry_msgs::msg::Point & back_collision_point, const double lat_dist_to_traj)
+{
+  if (!obstacle_filtering_param_.use_pointcloud) {
+    return std::nullopt;
+  }
+  const unique_identifier_msgs::msg::UUID obj_uuid;
+  const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid);
+
+  ObjectClassification unknown_object_classification;
+  unknown_object_classification.label = ObjectClassification::UNKNOWN;
+  unknown_object_classification.probability = 1.0;
+
+  const geometry_msgs::msg::Pose unconfigured_pose;
+
+  const double unconfigured_lon_velocity = 0.;
+  const double unconfigured_lat_velocity = 0.;
+
+  return SlowDownObstacle{
+    obj_uuid_str,
+    stamp,
+    unknown_object_classification,
+    unconfigured_pose,
+    unconfigured_lon_velocity,
+    unconfigured_lat_velocity,
+    lat_dist_to_traj,
+    front_collision_point,
+    back_collision_point};
+}
+
 std::vector<SlowdownInterval> ObstacleSlowDownModule::plan_slow_down(
   const std::shared_ptr<const PlannerData> planner_data,
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<SlowDownObstacle> & obstacles,

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp

@@ -28,6 +28,15 @@
 #include <autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp>
 #include <autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <pcl/common/transforms.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/segmentation/euclidean_cluster_comparator.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2_ros/buffer.h>
 
 #include <algorithm>
 #include <iostream>
@@ -82,18 +91,30 @@ class ObstacleSlowDownModule : public PluginModuleInterface
   mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
   mutable std::optional<std::vector<Polygon2d>> decimated_traj_polys_{std::nullopt};
 
+  std::vector<autoware::motion_velocity_planner::SlowDownPointData>
+  convert_point_cloud_to_slow_down_points(
+    const PlannerData::Pointcloud & pointcloud, const std::vector<TrajectoryPoint> & traj_points,
+    const VehicleInfo & vehicle_info, const size_t ego_idx);
   std::vector<SlowDownObstacle> filter_slow_down_obstacle_for_predicted_object(
     const Odometry & odometry, const double ego_nearest_dist_threshold,
     const double ego_nearest_yaw_threshold, const std::vector<TrajectoryPoint> & traj_points,
     const std::vector<TrajectoryPoint> & decimated_traj_points,
     const std::vector<std::shared_ptr<PlannerData::Object>> & objects,
     const rclcpp::Time & predicted_objects_stamp, const VehicleInfo & vehicle_info,
     const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check);
+  std::vector<SlowDownObstacle> filter_slow_down_obstacle_for_point_cloud(
+    const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+    const std::vector<TrajectoryPoint> & decimated_traj_points,
+    const PlannerData::Pointcloud & point_cloud, const VehicleInfo & vehicle_info,
+    const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check, size_t ego_idx);
   std::optional<SlowDownObstacle> create_slow_down_obstacle_for_predicted_object(
     const std::vector<TrajectoryPoint> & traj_points,
     const std::vector<Polygon2d> & decimated_traj_polys_with_lat_margin,
     const std::shared_ptr<PlannerData::Object> object, const rclcpp::Time & predicted_objects_stamp,
     const double dist_from_obj_poly_to_traj_poly);
+  std::optional<SlowDownObstacle> create_slow_down_obstacle_for_point_cloud(
+    const rclcpp::Time & stamp, const geometry_msgs::msg::Point & front_collision_point,
+    const geometry_msgs::msg::Point & back_collision_point, const double lat_dist_to_traj);
   std::vector<SlowdownInterval> plan_slow_down(
     const std::shared_ptr<const PlannerData> planner_data,
     const std::vector<TrajectoryPoint> & traj_points,

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/parameters.hpp

@@ -15,6 +15,7 @@
 #ifndef PARAMETERS_HPP_
 #define PARAMETERS_HPP_
 
+#include "autoware/motion_utils/trajectory/conversion.hpp"
 #include "type_alias.hpp"
 #include "types.hpp"
 
@@ -55,10 +56,24 @@ struct CommonParam
 
 struct ObstacleFilteringParam
 {
+  struct PointcloudObstacleFilteringParam
+  {
+    double pointcloud_voxel_grid_x{};
+    double pointcloud_voxel_grid_y{};
+    double pointcloud_voxel_grid_z{};
+    double pointcloud_cluster_tolerance{};
+    double pointcloud_min_cluster_size{};
+    double pointcloud_max_cluster_size{};
+  };
+
+  PointcloudObstacleFilteringParam pointcloud_obstacle_filtering_param;
   std::vector<uint8_t> object_types{};
 
+  bool use_pointcloud{false};
+
   double min_lat_margin{};
   double max_lat_margin{};
+
   double lat_hysteresis_margin{};
 
   int successive_num_to_entry_slow_down_condition{};
@@ -67,18 +82,35 @@ struct ObstacleFilteringParam
   ObstacleFilteringParam() = default;
   explicit ObstacleFilteringParam(rclcpp::Node & node)
   {
+    use_pointcloud = getOrDeclareParameter<bool>(
+      node, "obstacle_slow_down.obstacle_filtering.object_type.pointcloud");
     object_types =
       utils::get_target_object_type(node, "obstacle_slow_down.obstacle_filtering.object_type.");
     min_lat_margin =
       getOrDeclareParameter<double>(node, "obstacle_slow_down.obstacle_filtering.min_lat_margin");
     max_lat_margin =
       getOrDeclareParameter<double>(node, "obstacle_slow_down.obstacle_filtering.max_lat_margin");
+
     lat_hysteresis_margin = getOrDeclareParameter<double>(
       node, "obstacle_slow_down.obstacle_filtering.lat_hysteresis_margin");
     successive_num_to_entry_slow_down_condition = getOrDeclareParameter<int>(
       node, "obstacle_slow_down.obstacle_filtering.successive_num_to_entry_slow_down_condition");
     successive_num_to_exit_slow_down_condition = getOrDeclareParameter<int>(
       node, "obstacle_slow_down.obstacle_filtering.successive_num_to_exit_slow_down_condition");
+
+    pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_x = getOrDeclareParameter<double>(
+      node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_voxel_grid_x");
+    pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_y = getOrDeclareParameter<double>(
+      node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_voxel_grid_y");
+    pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_z = getOrDeclareParameter<double>(
+      node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_voxel_grid_z");
+    pointcloud_obstacle_filtering_param.pointcloud_cluster_tolerance =
+      getOrDeclareParameter<double>(
+        node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_cluster_tolerance");
+    pointcloud_obstacle_filtering_param.pointcloud_min_cluster_size = getOrDeclareParameter<int>(
+      node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_min_cluster_size");
+    pointcloud_obstacle_filtering_param.pointcloud_max_cluster_size = getOrDeclareParameter<int>(
+      node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_max_cluster_size");
   }
 };
 

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/types.hpp

@@ -18,12 +18,28 @@
 #include "type_alias.hpp"
 
 #include <algorithm>
+#include <optional>
 #include <string>
 #include <unordered_map>
 #include <vector>
 
 namespace autoware::motion_velocity_planner
 {
+
+struct SlowDownPointData
+{
+  std::optional<geometry_msgs::msg::Point> front{std::nullopt};
+  std::optional<geometry_msgs::msg::Point> back{std::nullopt};
+
+  double lat_dist_to_traj{0};
+
+  SlowDownPointData(
+    const std::optional<geometry_msgs::msg::Point> & front_point,
+    const std::optional<geometry_msgs::msg::Point> & back_point, double lat_dist_to_traj)
+  : front(front_point), back(back_point), lat_dist_to_traj(lat_dist_to_traj)
+  {
+  }
+};
 struct SlowDownObstacle
 {
   SlowDownObstacle(

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml

@@ -54,6 +54,14 @@
             bicycle: false
             pedestrian: false
 
+        pointcloud:
+          pointcloud_voxel_grid_x: 0.05
+          pointcloud_voxel_grid_y: 0.05
+          pointcloud_voxel_grid_z: 100000.0
+          pointcloud_cluster_tolerance: 1.0
+          pointcloud_min_cluster_size: 1
+          pointcloud_max_cluster_size: 100000
+
         # hysteresis for velocity
         obstacle_velocity_threshold_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
         obstacle_velocity_threshold_from_stop : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]