feat(obstacle_cruise_planner): support pointcloud-based obstacles (#6907)

* add pointcloud to obstacle properties

* add tf listener & pointcloud subscriber

* add parameters for pointcloud obstacle

* add type aliases

* convert pointcloud to obstacle

* add type alias

* add polygon conversion for pointcloud obstacle

* initialize twist & pose of pointcloud obstacle

* overload to handle both obstacle & predicted path

* implement ego behavior determination against pointcloud obstacles

* generate obstacle from point

* revert getCollisionIndex()

* generate obstacle from each point in cloud

* set pointcloud obstacle velocity to 0

* use tf buffer & listener with pointers

* update latest pointcloud data

* add topic remap

* remove unnecessary includes

* set slow down obstacle velocity to 0

* add flag to consider pointcloud obstacle for stopping & slowing down

* style(pre-commit): autofix

* downsample pointcloud using voxel grid

* change  shape type of pointcloud obstacle to polygon

* convert pointcloud to obstacle by clustering

* add parameters for clustering

* add max_num_points parameter to dummy object

* downsample pointcloud when the number of points is larger than max_num_points

* add max_num_points property to dummy bus

* add parameters for pointcloud based obstacles

* store pointcloud in obstacle struct

* change obstacle conversion method

* migrate previous changes to new package

* store necessary points only

* move use_pointcloud to common parameter

* extract necessary points from pointcloud

* add use_pointcloud parameter to planner interface

* fix obstacle conversion

* fix collision point determination

* simplify pointcloud transformation

* style(pre-commit): autofix

* fix collision point determination

* pick nearest stop collision point

* check collision for every point in cluster

* migrate previous changes to new files

* reduce diff

* remove use_pointcloud parameter

* add parameters for pointcloud filtering

* add autoware namespace

* Revert "add max_num_points parameter to dummy object"

This reverts commit 98bcd08.

* Revert "downsample pointcloud when the number of points is larger than max_num_points"

This reverts commit fb00b59.

* Revert "add max_num_points property to dummy bus"

This reverts commit 5f9e4ab.

* feat(diagnostic_graph_utils): add logging tool

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix all OK

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(default_ad_api): add log when operation mode change fails

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* get only the necessary one of object or pointcloud data

* addfield for obstacle source type

* enable simultaneous use of PredictedObjects and PointCloud

* separate convertToObstacles() by source type

* avoid using pointer

* reduce diff

* make nest shallower

* define vector concatenate function

* shorten variable names

* fix redundant condition

---------

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>

Author: 4 people

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,16 +61,27 @@ 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(
+  determineEgoBehaviorAgainstPredictedObjectObstacles(
     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>>
+  determineEgoBehaviorAgainstPointCloudObstacles(
+    const Odometry & odometry, 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(
+  std::optional<StopObstacle> createStopObstacleForPredictedObject(
     const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
     const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
     const double precise_lateral_dist) const;
+  std::optional<StopObstacle> createStopObstacleForPointCloud(
+    const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
+    const double precise_lateral_dist) const;
   bool isStopObstacle(const uint8_t label) const;
   bool isInsideCruiseObstacle(const uint8_t label) const;
   bool isOutsideCruiseObstacle(const uint8_t label) const;
@@ -94,9 +108,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   double calcCollisionTimeMargin(
     const Odometry & odometry, const std::vector<PointWithStamp> & collision_points,
     const std::vector<TrajectoryPoint> & traj_points, const bool is_driving_forward) const;
-  std::optional<SlowDownObstacle> createSlowDownObstacle(
+  std::optional<SlowDownObstacle> createSlowDownObstacleForPredictedObject(
     const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
     const Obstacle & obstacle, const double precise_lat_dist);
+  std::optional<SlowDownObstacle> createSlowDownObstacleForPointCloud(
+    const Obstacle & obstacle, const double precise_lat_dist);
   PlannerData createPlannerData(
     const Odometry & odometry, const AccelWithCovarianceStamped & acc,
     const std::vector<TrajectoryPoint> & traj_points) const;
@@ -116,6 +132,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
 
   bool enable_debug_info_;
   bool enable_calculation_time_info_;
+  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 +167,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 +205,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;
@@ -271,6 +301,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
 
   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_