feat(obstacle_cruise_planner): support pointcloud-based obstacles

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -25,6 +25,10 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <boost/geometry.hpp>
+
+#include <pcl/common/centroid.h>
+
 #include <optional>
 #include <string>
 #include <vector>
@@ -66,15 +70,41 @@ struct Obstacle
     uuid(tier4_autoware_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)
+    lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj),
+    pointcloud_repr(false)
   {
     predicted_paths.clear();
     for (const auto & path : object.kinematics.predicted_paths) {
       predicted_paths.push_back(path);
     }
   }
 
-  Polygon2d toPolygon() const { return tier4_autoware_utils::toPolygon2d(pose, shape); }
+  Obstacle(const rclcpp::Time & arg_stamp, const PointCloud & object)
+  : stamp(arg_stamp), uuid(""), pointcloud(object), pointcloud_repr(true)
+  {
+    twist.linear.x = twist.linear.y = twist.angular.z = 0.0;
+
+    Eigen::Vector4d centroid;
+    pcl::compute3DCentroid(pointcloud, centroid);
+    pose.position.x = centroid.x();
+    pose.position.y = centroid.y();
+    pose.position.z = centroid.z();
+  }
+
+  Polygon2d toPolygon() const
+  {
+    if (pointcloud_repr) {
+      MultiPoint2d points;
+      for (const auto & point : pointcloud) {
+        bg::append(points, Point2d(point.x, point.y));
+      }
+      Polygon2d polygon;
+      bg::convex_hull(points, polygon);
+      return polygon;
+    } else {
+      return tier4_autoware_utils::toPolygon2d(pose, shape);
+    }
+  }
 
   rclcpp::Time stamp;  // This is not the current stamp, but when the object was observed.
   geometry_msgs::msg::Pose pose;  // interpolated with the current stamp
@@ -87,6 +117,8 @@ struct Obstacle
   std::vector<PredictedPath> predicted_paths;
   double ego_to_obstacle_distance;
   double lat_dist_from_obstacle_to_traj;
+  PointCloud pointcloud;
+  bool pointcloud_repr;
 };
 
 struct TargetObstacleInterface

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -27,6 +27,9 @@
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
 #include <algorithm>
 #include <memory>
 #include <mutex>
@@ -55,7 +58,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     const std::vector<TrajectoryPoint> & traj_points,
     const vehicle_info_util::VehicleInfo & vehicle_info,
     const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const;
-  std::vector<Obstacle> convertToObstacles(const std::vector<TrajectoryPoint> & traj_points) const;
+  std::vector<Obstacle> convertToObstacles(
+    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 std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles);
@@ -143,9 +148,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     this, "~/input/odometry"};
   tier4_autoware_utils::InterProcessPollingSubscriber<PredictedObjects> objects_sub_{
     this, "~/input/objects"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<PointCloud2> pointcloud_sub_{
+    this, "~/input/pointcloud"};
   tier4_autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> acc_sub_{
     this, "~/input/acceleration"};
 
+  tf2_ros::Buffer tf_buffer_{get_clock()};
+  tf2_ros::TransformListener tf_listener_{tf_buffer_};
+
   // Vehicle Parameters
   VehicleInfo vehicle_info_;
 
@@ -176,6 +186,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     void onParam(const std::vector<rclcpp::Parameter> & parameters);
 
     double decimate_trajectory_step_length;
+    bool use_pointcloud{false};
+    double pointcloud_search_radius;
     // hysteresis for stop and cruise
     double obstacle_velocity_threshold_from_cruise_to_stop;
     double obstacle_velocity_threshold_from_stop_to_cruise;

planning/obstacle_cruise_planner/include/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_adapi_v1_msgs::msg::PlanningBehavior;
 using autoware_adapi_v1_msgs::msg::VelocityFactor;
 using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
@@ -49,6 +53,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;
@@ -60,7 +65,10 @@ using vehicle_info_util::VehicleInfo;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 namespace bg = boost::geometry;
+using tier4_autoware_utils::MultiPoint2d;
 using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
 
+using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+
 #endif  // OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_