feat(autonomous_emergency_braking): speed up aeb (autowarefoundation#…

…8778)

* add missing rclcpp::Time(0)

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* refactor to reduce cropping to once per iteration

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add LookUpTransform to utils

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* separate object creation and clustering

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* error handling of empty pointcloud

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

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

@@ -410,6 +410,16 @@ class AEB : public rclcpp::Node
    */
   std::optional<Path> generateEgoPath(const Trajectory & predicted_traj);
 
+  /**
+   * @brief Generate the footprint of the path with extra width margin
+   * @param path Ego vehicle path
+   * @param extra_width_margin Extra width margin for the footprint
+   * @param polygons vector to be filled with the polygons
+   * @return Vector of polygons representing the path footprint
+   */
+  void generatePathFootprint(
+    const Path & path, const double extra_width_margin, std::vector<Polygon2d> & polygons);
+
   /**
    * @brief Generate the footprint of the path with extra width margin
    * @param path Ego vehicle path
@@ -426,10 +436,19 @@ class AEB : public rclcpp::Node
    * @param objects Vector to store the created object data
    * @param obstacle_points_ptr Pointer to the point cloud of obstacles
    */
-  void createObjectDataUsingPointCloudClusters(
+  void getClosestObjectsOnPath(
     const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
-    std::vector<ObjectData> & objects,
-    const pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_ptr);
+    const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector<ObjectData> & objects);
+
+  /**
+   * @brief Create object data using point cloud clusters
+   * @param obstacle_points_ptr Pointer to the point cloud of obstacles
+   * @param points_belonging_to_cluster_hulls output: pointer to the point cloud of points belonging
+   * to cluster hulls
+   */
+  void getPointsBelongingToClusterHulls(
+    const PointCloud::Ptr obstacle_points_ptr,
+    const PointCloud::Ptr points_belonging_to_cluster_hulls);
 
   /**
    * @brief Create object data using predicted objects

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

@@ -24,6 +24,8 @@
 #include <boost/geometry/algorithms/correct.hpp>
 
 #include <tf2/utils.h>
+
+#include <string>
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -50,7 +52,7 @@ using geometry_msgs::msg::TransformStamped;
  * @param transform_stamped the tf2 transform
  */
 PredictedObject transformObjectFrame(
-  const PredictedObject & input, geometry_msgs::msg::TransformStamped & transform_stamped);
+  const PredictedObject & input, const geometry_msgs::msg::TransformStamped & transform_stamped);
 
 /**
  * @brief Get the predicted objects polygon as a geometry polygon
@@ -82,6 +84,10 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon(
  * @param obj the object
  */
 Polygon2d convertObjToPolygon(const PredictedObject & obj);
+
+std::optional<geometry_msgs::msg::TransformStamped> getTransform(
+  const std::string & target_frame, const std::string & source_frame,
+  const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger);
 }  // namespace autoware::motion::control::autonomous_emergency_braking::utils
 
 #endif  // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_