tmp: not build passed

Author: takayuki5168

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp

@@ -23,56 +23,26 @@
 #include "autoware/universe_utils/ros/update_param.hpp"
 #include "autoware/universe_utils/ros/uuid_helper.hpp"
 
+#include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <tier4_metric_msgs/msg/metric.hpp>
 #include <tier4_metric_msgs/msg/metric_array.hpp>
 
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl_conversions/pcl_conversions.h>
+
 #include <optional>
 #include <string>
 #include <vector>
 
 using Metric = tier4_metric_msgs::msg::Metric;
 using MetricArray = tier4_metric_msgs::msg::MetricArray;
 
-struct PlannerData
-{
-  rclcpp::Time current_time;
-  std::vector<TrajectoryPoint> traj_points;
-  nav_msgs::msg::Odometry current_odometry;
-  geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration;
-  // geometry_msgs::msg::Pose ego_pose;
-  // double ego_vel;
-  // double ego_acc;
-  bool is_driving_forward;
-  VehicleInfo vehicle_info;
-  double ego_nearest_dist_threshold;
-  double ego_nearest_yaw_threshold;
-
-  size_t findIndex(
-    const std::vector<TrajectoryPoint> & traj_points, const geometry_msgs::msg::Pose & pose) const
-  {
-    return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(
-      traj_points, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
-  }
-
-  size_t findSegmentIndex(
-    const std::vector<TrajectoryPoint> & traj_points, const geometry_msgs::msg::Pose & pose) const
-  {
-    return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-      traj_points, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
-  }
-};
-
-struct PointWithStamp
+struct PredictedObjectBasedObstacle
 {
-  rclcpp::Time stamp;
-  geometry_msgs::msg::Point point;
-};
-
-struct Obstacle
-{
-  Obstacle(
+  PredictedObjectBasedObstacle(
     const rclcpp::Time & arg_stamp, const PredictedObject & object,
     const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance,
     const double lat_dist_from_obstacle_to_traj, const double longitudinal_velocity,
@@ -97,22 +67,6 @@ struct Obstacle
     }
   }
 
-  Obstacle(
-    const rclcpp::Time & arg_stamp,
-    const std::optional<geometry_msgs::msg::Point> & stop_collision_point,
-    const std::optional<geometry_msgs::msg::Point> & slow_down_front_collision_point,
-    const std::optional<geometry_msgs::msg::Point> & slow_down_back_collision_point,
-    const double ego_to_obstacle_distance, const double lat_dist_from_obstacle_to_traj)
-  : stamp(arg_stamp),
-    ego_to_obstacle_distance(ego_to_obstacle_distance),
-    lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj),
-    precise_lat_dist(lat_dist_from_obstacle_to_traj),
-    stop_collision_point(stop_collision_point),
-    slow_down_front_collision_point(slow_down_front_collision_point),
-    slow_down_back_collision_point(slow_down_back_collision_point)
-  {
-  }
-
   rclcpp::Time stamp;  // This is not the current stamp, but when the object was observed.
   double ego_to_obstacle_distance;
   double lat_dist_from_obstacle_to_traj;
@@ -129,11 +83,55 @@ struct Obstacle
   Shape shape;
   double precise_lat_dist;
   std::vector<PredictedPath> predicted_paths;
+};
+
+struct PointcloudBasedObstacle
+{
+  PointcloudBasedObstacle(const rclcpp::Time & arg_stamp, const pcl::PointIndices & arg_clusters)
+  : stamp(arg_stamp), clusters(arg_clusters)
+  {
+  }
+
+  rclcpp::Time stamp;  // This is not the current stamp, but when the object was observed.
+  pcl::PointIndices clusters;
+};
+
+struct PlannerData
+{
+  rclcpp::Time current_time;
+  std::vector<TrajectoryPoint> traj_points;
+  nav_msgs::msg::Odometry current_odometry;
+  geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration;
+  // geometry_msgs::msg::Pose ego_pose;
+  // double ego_vel;
+  // double ego_acc;
+  bool is_driving_forward;
+  VehicleInfo vehicle_info;
+  double ego_nearest_dist_threshold;
+  double ego_nearest_yaw_threshold;
+
+  std::vector<PredictedObjectBasedObstacle> predicted_object_based_obstacles;
+  std::vector<PointcloudBasedObstacle> pointcloud_based_obstacles;
 
-  // for PointCloud
-  std::optional<geometry_msgs::msg::Point> stop_collision_point;
-  std::optional<geometry_msgs::msg::Point> slow_down_front_collision_point;
-  std::optional<geometry_msgs::msg::Point> slow_down_back_collision_point;
+  size_t findIndex(
+    const std::vector<TrajectoryPoint> & traj_points, const geometry_msgs::msg::Pose & pose) const
+  {
+    return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(
+      traj_points, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
+  }
+
+  size_t findSegmentIndex(
+    const std::vector<TrajectoryPoint> & traj_points, const geometry_msgs::msg::Pose & pose) const
+  {
+    return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+      traj_points, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
+  }
+};
+
+struct PointWithStamp
+{
+  rclcpp::Time stamp;
+  geometry_msgs::msg::Point point;
 };
 
 struct LongitudinalInfo

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/cruise/obstacle_cruise_module.hpp

@@ -316,7 +316,8 @@ class ObstacleCruiseModule
   }
 
   std::vector<TrajectoryPoint> plan(
-    const std::vector<TrajectoryPoint> & base_traj_points, const std::vector<Obstacle> & obstacles,
+    const std::vector<TrajectoryPoint> & base_traj_points,
+    const std::vector<PredictedObjectBasedObstacle> & obstacles,
     const CommonBehaviorDeterminationParam & common_behavior_determination_param,
     const PlannerData & planner_data, const std::vector<TrajectoryPoint> & stop_traj_points)
   {
@@ -359,8 +360,9 @@ class ObstacleCruiseModule
 
   std::vector<CruiseObstacle> determineEgoBehaviorAgainstPredictedObjectObstacles(
     const Odometry & odometry, const std::vector<TrajectoryPoint> & decimated_traj_points,
-    const std::vector<Polygon2d> & decimated_traj_polys, const std::vector<Obstacle> & obstacles,
-    const bool is_driving_forward, const VehicleInfo & vehicle_info)
+    const std::vector<Polygon2d> & decimated_traj_polys,
+    const std::vector<PredictedObjectBasedObstacle> & obstacles, const bool is_driving_forward,
+    const VehicleInfo & vehicle_info)
   {
     // cruise
     std::vector<CruiseObstacle> cruise_obstacles;
@@ -558,15 +560,15 @@ class ObstacleCruiseModule
   struct DebugData
   {
     DebugData() = default;
-    std::vector<Obstacle> intentionally_ignored_obstacles;
+    std::vector<PredictedObjectBasedObstacle> intentionally_ignored_obstacles;
     std::vector<CruiseObstacle> obstacles_to_cruise;
     MarkerArray cruise_wall_marker;
     std::optional<std::vector<Metric>> cruise_metrics{std::nullopt};
   };
 
   std::optional<CruiseObstacle> createCruiseObstacle(
     const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
-    const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
+    const std::vector<Polygon2d> & traj_polys, const PredictedObjectBasedObstacle & obstacle,
     const double precise_lat_dist, const bool is_driving_forward, const VehicleInfo & vehicle_info)
   {
     const auto & object_id = obstacle.uuid.substr(0, 4);
@@ -633,14 +635,14 @@ class ObstacleCruiseModule
   }
 
   std::optional<std::vector<CruiseObstacle>> findYieldCruiseObstacles(
-    const std::vector<Obstacle> & obstacles, const std::vector<TrajectoryPoint> & traj_points,
-    const VehicleInfo & vehicle_info)
+    const std::vector<PredictedObjectBasedObstacle> & obstacles,
+    const std::vector<TrajectoryPoint> & traj_points, const VehicleInfo & vehicle_info)
   {
     if (obstacles.empty() || traj_points.empty()) return std::nullopt;
     const auto & p = cruise_behavior_determination_param_;
 
-    std::vector<Obstacle> stopped_obstacles;
-    std::vector<Obstacle> moving_obstacles;
+    std::vector<PredictedObjectBasedObstacle> stopped_obstacles;
+    std::vector<PredictedObjectBasedObstacle> moving_obstacles;
 
     std::for_each(
       obstacles.begin(), obstacles.end(),
@@ -715,7 +717,7 @@ class ObstacleCruiseModule
 
   std::optional<std::vector<PointWithStamp>> createCollisionPointsForInsideCruiseObstacle(
     const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
-    const Obstacle & obstacle, const bool is_driving_forward,
+    const PredictedObjectBasedObstacle & obstacle, const bool is_driving_forward,
     const VehicleInfo & vehicle_info) const
   {
     const auto & object_id = obstacle.uuid.substr(0, 4);
@@ -777,7 +779,7 @@ class ObstacleCruiseModule
 
   std::optional<std::vector<PointWithStamp>> createCollisionPointsForOutsideCruiseObstacle(
     const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
-    const Obstacle & obstacle, const bool is_driving_forward,
+    const PredictedObjectBasedObstacle & obstacle, const bool is_driving_forward,
     const VehicleInfo & vehicle_info) const
   {
     const auto & cp = common_behavior_determination_param_;
@@ -866,7 +868,7 @@ class ObstacleCruiseModule
   }
 
   std::optional<CruiseObstacle> createYieldCruiseObstacle(
-    const Obstacle & obstacle, const std::vector<TrajectoryPoint> & traj_points)
+    const PredictedObjectBasedObstacle & obstacle, const std::vector<TrajectoryPoint> & traj_points)
   {
     if (traj_points.empty()) return std::nullopt;
     // check label
@@ -946,7 +948,7 @@ class ObstacleCruiseModule
   }
 
   bool isFrontCollideObstacle(
-    const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
+    const std::vector<TrajectoryPoint> & traj_points, const PredictedObjectBasedObstacle & obstacle,
     const size_t first_collision_idx) const
   {
     const auto obstacle_idx =
@@ -961,7 +963,8 @@ class ObstacleCruiseModule
   }
 
   bool isObstacleCrossing(
-    const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle) const
+    const std::vector<TrajectoryPoint> & traj_points,
+    const PredictedObjectBasedObstacle & obstacle) const
   {
     const double diff_angle = calcDiffAngleAgainstTrajectory(traj_points, obstacle.pose);
 

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp

@@ -56,18 +56,18 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg);
 
   // main functions
-  std::vector<Obstacle> convertToObstacles(
+  std::vector<PredictedObjectBasedObstacle> convertToPredictedObjectBasedObstacles(
     const Odometry & odometry, const PredictedObjects & objects,
     const std::vector<TrajectoryPoint> & traj_points, const PlannerData & planner_data) const;
-  std::vector<Obstacle> convertToObstacles(
+  std::vector<PointcloudBasedObstacle> convertToPointcloudBasedObstacles(
     const Odometry & odometry, const PointCloud2 & pointcloud,
     const std::vector<TrajectoryPoint> & traj_points, const std_msgs::msg::Header & traj_header,
     const PlannerData & planner_data) const;
   /*
 std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
   determineEgoBehaviorAgainstPointCloudObstacles(
     const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
-    const std::vector<Obstacle> & obstacles);
+    const std::vector<PredictedObjectBasedObstacle> & obstacles);
   */
   std::vector<TrajectoryPoint> decimateTrajectoryPoints(
     const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
@@ -115,9 +115,6 @@ std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<S
   // planner
   std::unique_ptr<ObstacleCruiseModule> planner_ptr_{nullptr};
 
-  // PointCloud-based stop obstacle history
-  // std::vector<StopObstacle> stop_pc_obstacle_history_;
-
   // behavior determination parameter
   CommonBehaviorDeterminationParam common_behavior_determination_param_;
 

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp

@@ -40,7 +40,7 @@ Polygon2d createOneStepPolygon(
 
 std::optional<std::pair<geometry_msgs::msg::Point, double>> getCollisionPoint(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
-  const Obstacle & obstacle, const double dist_to_bumper);
+  const PredictedObjectBasedObstacle & obstacle, const double dist_to_bumper);
 
 std::optional<std::pair<geometry_msgs::msg::Point, double>> getCollisionPoint(
   const std::vector<TrajectoryPoint> & traj_points, const size_t collision_idx,

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/slow_down/obstacle_slow_down_module.hpp

@@ -290,7 +290,7 @@ class ObstacleSlowDownModule
   }
 
   std::vector<TrajectoryPoint> plan(
-    const std::vector<TrajectoryPoint> & base_traj_points, const std::vector<Obstacle> & obstacles,
+    const std::vector<TrajectoryPoint> & base_traj_points,
     const CommonBehaviorDeterminationParam & common_behavior_determination_param,
     const PlannerData & planner_data, const std::vector<TrajectoryPoint> & cruise_traj_points)
   {
@@ -302,7 +302,8 @@ class ObstacleSlowDownModule
       common_behavior_determination_param_.decimate_trajectory_step_length, 0.0);
 
     const auto slow_down_obstacles = determineEgoBehaviorAgainstPredictedObjectObstacles(
-      planner_data.current_odometry, decimated_traj_points, obstacles, planner_data.vehicle_info);
+      planner_data.current_odometry, decimated_traj_points, planner_data.obstacles,
+      planner_data.vehicle_info);
     std::optional<VelocityLimit> slow_down_vel_limit;
     const auto slow_down_traj_points = generateSlowDownTrajectory(
       planner_data, cruise_traj_points, slow_down_obstacles, slow_down_vel_limit,
@@ -324,7 +325,7 @@ class ObstacleSlowDownModule
 private:
   std::vector<SlowDownObstacle> determineEgoBehaviorAgainstPredictedObjectObstacles(
     const Odometry & odometry, const std::vector<TrajectoryPoint> & decimated_traj_points,
-    const std::vector<Obstacle> & obstacles, const VehicleInfo & vehicle_info)
+    const std::vector<PredictedObjectBasedObstacle> & obstacles, const VehicleInfo & vehicle_info)
   {
     // slow down
     slow_down_condition_counter_.resetCurrentUuids();
@@ -885,7 +886,8 @@ class ObstacleSlowDownModule
 
   std::optional<SlowDownObstacle> createSlowDownObstacleForPredictedObject(
     const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
-    const Obstacle & obstacle, const double precise_lat_dist, const VehicleInfo & vehicle_info)
+    const PredictedObjectBasedObstacle & obstacle, const double precise_lat_dist,
+    const VehicleInfo & vehicle_info)
   {
     const auto & object_id = obstacle.uuid.substr(0, 4);
     const auto & p = slow_down_param_;
@@ -1016,7 +1018,7 @@ class ObstacleSlowDownModule
   }
 
   std::optional<SlowDownObstacle> createSlowDownObstacleForPointCloud(
-    const Obstacle & obstacle, const double precise_lat_dist)
+    const PredictedObjectBasedObstacle & obstacle, const double precise_lat_dist)
   {
     if (!enable_slow_down_planning_ || !use_pointcloud_for_slow_down_) {
       return std::nullopt;