feat(autonomous_emergency_braking): make hull markers 3d (#8930)

make hull markers 3d

Signed-off-by: Daniel Sanchez <[email protected]>

control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp

@@ -65,6 +65,7 @@ using sensor_msgs::msg::Imu;
 using sensor_msgs::msg::PointCloud2;
 using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
 using autoware::universe_utils::Polygon2d;
+using autoware::universe_utils::Polygon3d;
 using autoware::vehicle_info_utils::VehicleInfo;
 using diagnostic_updater::DiagnosticStatusWrapper;
 using diagnostic_updater::Updater;
@@ -496,7 +497,7 @@ class AEB : public rclcpp::Node
    * @param debug_markers Marker array for debugging
    */
   void addClusterHullMarkers(
-    const rclcpp::Time & current_time, const std::vector<Polygon2d> & hulls,
+    const rclcpp::Time & current_time, const std::vector<Polygon3d> & hulls,
     const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers);
 
   /**

control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp

@@ -42,6 +42,7 @@
 namespace autoware::motion::control::autonomous_emergency_braking::utils
 {
 using autoware::universe_utils::Polygon2d;
+using autoware::universe_utils::Polygon3d;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjects;
 using geometry_msgs::msg::Point;
@@ -105,6 +106,14 @@ std::optional<geometry_msgs::msg::TransformStamped> getTransform(
  */
 void fillMarkerFromPolygon(
   const std::vector<Polygon2d> & polygons, visualization_msgs::msg::Marker & polygon_marker);
+
+/**
+ * @brief Get the predicted object's shape as a geometry polygon
+ * @param polygons vector of Polygon3d
+ * @param polygon_marker marker to be filled with polygon points
+ */
+void fillMarkerFromPolygon(
+  const std::vector<Polygon3d> & polygons, visualization_msgs::msg::Marker & polygon_marker);
 }  // namespace autoware::motion::control::autonomous_emergency_braking::utils
 
 #endif  // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_

control/autoware_autonomous_emergency_braking/src/node.cpp

@@ -59,6 +59,7 @@ namespace autoware::motion::control::autonomous_emergency_braking
 {
 using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon;
 using autoware::universe_utils::Point2d;
+using autoware::universe_utils::Point3d;
 using diagnostic_msgs::msg::DiagnosticStatus;
 namespace bg = boost::geometry;
 
@@ -71,6 +72,16 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point &
   bg::append(polygon.outer(), point);
 }
 
+void appendPointToPolygon(Polygon3d & polygon, const geometry_msgs::msg::Point & geom_point)
+{
+  Point3d point;
+  point.x() = geom_point.x;
+  point.y() = geom_point.y;
+  point.z() = geom_point.z;
+
+  bg::append(polygon.outer(), point);
+}
+
 Polygon2d createPolygon(
   const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose,
   const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width)
@@ -580,7 +591,6 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object
   // collision happens
   ObjectData collision_data = closest_object;
   collision_data.rss = rss_dist;
-  collision_data.distance_to_object = closest_object.distance_to_object;
   collision_data_keeper_.setCollisionData(collision_data);
   return true;
 }
@@ -787,7 +797,7 @@ void AEB::getPointsBelongingToClusterHulls(
     ec.extract(cluster_idx);
     return cluster_idx;
   });
-  std::vector<Polygon2d> hull_polygons;
+  std::vector<Polygon3d> hull_polygons;
   for (const auto & indices : cluster_indices) {
     PointCloud::Ptr cluster(new PointCloud);
     bool cluster_surpasses_threshold_height{false};
@@ -806,7 +816,7 @@ void AEB::getPointsBelongingToClusterHulls(
     std::vector<pcl::Vertices> polygons;
     PointCloud::Ptr surface_hull(new PointCloud);
     hull.reconstruct(*surface_hull, polygons);
-    Polygon2d hull_polygon;
+    Polygon3d hull_polygon;
     for (const auto & p : *surface_hull) {
       points_belonging_to_cluster_hulls->push_back(p);
       if (publish_debug_markers_) {
@@ -900,7 +910,7 @@ void AEB::cropPointCloudWithEgoFootprintPath(
 }
 
 void AEB::addClusterHullMarkers(
-  const rclcpp::Time & current_time, const std::vector<Polygon2d> & hulls,
+  const rclcpp::Time & current_time, const std::vector<Polygon3d> & hulls,
   const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

control/autoware_autonomous_emergency_braking/src/utils.cpp

@@ -186,4 +186,21 @@ void fillMarkerFromPolygon(
   }
 }
 
+void fillMarkerFromPolygon(
+  const std::vector<Polygon3d> & polygons, visualization_msgs::msg::Marker & polygon_marker)
+{
+  for (const auto & poly : polygons) {
+    for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) {
+      const auto & boost_cp = poly.outer().at(dp_idx);
+      const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size());
+      const auto curr_point =
+        autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), boost_cp.z());
+      const auto next_point =
+        autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), boost_np.z());
+      polygon_marker.points.push_back(curr_point);
+      polygon_marker.points.push_back(next_point);
+    }
+  }
+}
+
 }  // namespace autoware::motion::control::autonomous_emergency_braking::utils