feat(obstacle_cruise_planner): support pointcloud-based obstacles

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

@@ -160,6 +160,7 @@
           <remap from="~/input/odometry" to="/localization/kinematic_state"/>
           <remap from="~/input/acceleration" to="/localization/acceleration"/>
           <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
+          <remap from="~/input/pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
           <remap from="~/output/trajectory" to="$(var interface_output_topic)"/>
           <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
           <remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>

planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -37,6 +37,7 @@
         motorcycle: true
         bicycle: true
         pedestrian: true
+        pointcloud: false
 
       cruise_obstacle_type:
         inside:
@@ -68,8 +69,17 @@
         motorcycle: true
         bicycle: true
         pedestrian: true
+        pointcloud: false
 
     behavior_determination:
+      pointcloud_search_radius: 5.0
+      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
+
       decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking
 
       prediction_resampling_time_interval: 0.1

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

@@ -58,25 +58,42 @@ struct Obstacle
     const geometry_msgs::msg::Pose & arg_pose, 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),
     pose(arg_pose),
     orientation_reliable(true),
     twist(object.kinematics.initial_twist_with_covariance.twist),
     twist_reliable(true),
     classification(object.classification.at(0)),
     uuid(autoware::universe_utils::toHexString(object.object_id)),
-    shape(object.shape),
-    ego_to_obstacle_distance(ego_to_obstacle_distance),
-    lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj)
+    shape(object.shape)
   {
     predicted_paths.clear();
     for (const auto & path : object.kinematics.predicted_paths) {
       predicted_paths.push_back(path);
     }
   }
 
-  Polygon2d toPolygon() const { return autoware::universe_utils::toPolygon2d(pose, shape); }
+  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),
+    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;
+
+  // for PredictedObject
   geometry_msgs::msg::Pose pose;  // interpolated with the current stamp
   bool orientation_reliable;
   Twist twist;
@@ -85,8 +102,11 @@ struct Obstacle
   std::string uuid;
   Shape shape;
   std::vector<PredictedPath> predicted_paths;
-  double ego_to_obstacle_distance;
-  double lat_dist_from_obstacle_to_traj;
+
+  // 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;
 };
 
 struct TargetObstacleInterface

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

@@ -27,6 +27,9 @@
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
 #include <algorithm>
 #include <memory>
 #include <mutex>
@@ -58,10 +61,18 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   std::vector<Obstacle> convertToObstacles(
     const Odometry & odometry, const PredictedObjects & objects,
     const std::vector<TrajectoryPoint> & traj_points) const;
+  std::vector<Obstacle> convertToObstacles(
+    const Odometry & odometry, const PointCloud2 & pointcloud,
+    const std::vector<TrajectoryPoint> & traj_points,
+    const std_msgs::msg::Header & traj_header) const;
   std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
   determineEgoBehaviorAgainstObstacles(
     const Odometry & odometry, const PredictedObjects & objects,
     const std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles);
+  std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
+  determineEgoBehaviorAgainstObstacles(
+    const Odometry & odometry, const PointCloud2 & /* pointcloud */,
+    const std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles);
   std::vector<TrajectoryPoint> decimateTrajectoryPoints(
     const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points) const;
   std::optional<StopObstacle> createStopObstacle(
@@ -116,6 +127,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
 
   bool enable_debug_info_;
   bool enable_calculation_time_info_;
+  bool enable_slow_down_planning_{false};
+  bool use_pointcloud_for_stop_;
+  bool use_pointcloud_for_slow_down_;
   double min_behavior_stop_margin_;
   bool enable_approaching_on_curve_;
   double additional_safe_distance_margin_on_curve_;
@@ -149,9 +163,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     this, "~/input/odometry"};
   autoware::universe_utils::InterProcessPollingSubscriber<PredictedObjects> objects_sub_{
     this, "~/input/objects"};
+  autoware::universe_utils::InterProcessPollingSubscriber<PointCloud2> pointcloud_sub_{
+    this, "~/input/pointcloud"};
   autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> acc_sub_{
     this, "~/input/acceleration"};
 
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
+
   // Vehicle Parameters
   VehicleInfo vehicle_info_;
 
@@ -182,6 +201,13 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     void onParam(const std::vector<rclcpp::Parameter> & parameters);
 
     double decimate_trajectory_step_length;
+    double pointcloud_search_radius;
+    double pointcloud_voxel_grid_x;
+    double pointcloud_voxel_grid_y;
+    double pointcloud_voxel_grid_z;
+    double pointcloud_cluster_tolerance;
+    int pointcloud_min_cluster_size;
+    int pointcloud_max_cluster_size;
     // hysteresis for stop and cruise
     double obstacle_velocity_threshold_from_cruise_to_stop;
     double obstacle_velocity_threshold_from_stop_to_cruise;
@@ -270,7 +296,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   EgoNearestParam ego_nearest_param_;
 
   bool is_driving_forward_{true};
-  bool enable_slow_down_planning_{false};
+  bool use_pointcloud_{false};
 
   std::vector<StopObstacle> prev_closest_stop_obstacles_{};
 

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

@@ -63,12 +63,13 @@ class PlannerInterface
 
   void setParam(
     const bool enable_debug_info, const bool enable_calculation_time_info,
-    const double min_behavior_stop_margin, const double enable_approaching_on_curve,
-    const double additional_safe_distance_margin_on_curve,
+    const bool use_pointcloud, const double min_behavior_stop_margin,
+    const double enable_approaching_on_curve, const double additional_safe_distance_margin_on_curve,
     const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop)
   {
     enable_debug_info_ = enable_debug_info;
     enable_calculation_time_info_ = enable_calculation_time_info;
+    use_pointcloud_ = use_pointcloud;
     min_behavior_stop_margin_ = min_behavior_stop_margin;
     enable_approaching_on_curve_ = enable_approaching_on_curve;
     additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve;
@@ -117,6 +118,7 @@ class PlannerInterface
   // Parameters
   bool enable_debug_info_{false};
   bool enable_calculation_time_info_{false};
+  bool use_pointcloud_{false};
   LongitudinalInfo longitudinal_info_;
   double min_behavior_stop_margin_;
   bool enable_approaching_on_curve_;

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

@@ -27,6 +27,7 @@
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
 #include "tier4_debug_msgs/msg/float32_stamped.hpp"
 #include "tier4_planning_msgs/msg/stop_factor.hpp"
 #include "tier4_planning_msgs/msg/stop_reason_array.hpp"
@@ -35,6 +36,9 @@
 #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
 using autoware::vehicle_info_utils::VehicleInfo;
 using autoware_adapi_v1_msgs::msg::PlanningBehavior;
 using autoware_adapi_v1_msgs::msg::VelocityFactor;
@@ -50,6 +54,7 @@ using geometry_msgs::msg::AccelStamped;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using geometry_msgs::msg::Twist;
 using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::PointCloud2;
 using tier4_debug_msgs::msg::Float32Stamped;
 using tier4_planning_msgs::msg::StopFactor;
 using tier4_planning_msgs::msg::StopReason;
@@ -63,4 +68,6 @@ namespace bg = boost::geometry;
 using autoware::universe_utils::Point2d;
 using autoware::universe_utils::Polygon2d;
 
+using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+
 #endif  // AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_