feat(autonomous_emergency_braking): add predicted object support for …

…aeb (#7548)

* add polling sub to predicted objects

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

* WIP requires changing path frame to map

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

* add parameters and reuse predicted obj speed

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

* introduce early break to reduce computation time

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

* resolve merge conflicts

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

* fix guard

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

* remove unused declaration

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

* fix include

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

* fix include issues

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

* remove inline

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

* delete unused dependencies

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

* add utils.cpp

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

* remove _ for non member variable

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

---------

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

control/autoware_autonomous_emergency_braking/CMakeLists.txt

@@ -12,11 +12,18 @@ include_directories(
   ${PCL_INCLUDE_DIRS}
 )
 
+ament_auto_add_library(autoware_autonomous_emergency_braking_helpers SHARED
+  include/autoware/autonomous_emergency_braking/utils.hpp
+  src/utils.cpp
+)
+
 set(AEB_NODE ${PROJECT_NAME}_node)
 ament_auto_add_library(${AEB_NODE} SHARED
+  include/autoware/autonomous_emergency_braking/node.hpp
   src/node.cpp
 )
 
+target_link_libraries(${AEB_NODE} autoware_autonomous_emergency_braking_helpers)
 rclcpp_components_register_node(${AEB_NODE}
   PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB"
   EXECUTABLE ${PROJECT_NAME}

control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml

@@ -3,6 +3,8 @@
     # Ego path calculation
     use_predicted_trajectory: true
     use_imu_path: false
+    use_pointcloud_data: true
+    use_predicted_object_data: true
     use_object_velocity_calculation: true
     min_generated_path_length: 0.5
     imu_prediction_time_horizon: 1.5

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

@@ -23,6 +23,7 @@
 #include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <autoware_system_msgs/msg/autoware_state.hpp>
 #include <autoware_vehicle_msgs/msg/velocity_report.hpp>
@@ -68,6 +69,9 @@ using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 using Path = std::vector<geometry_msgs::msg::Pose>;
 using Vector3 = geometry_msgs::msg::Vector3;
+using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::PredictedObjects;
+
 struct ObjectData
 {
   rclcpp::Time stamp;
@@ -176,6 +180,13 @@ class CollisionDataKeeper
   std::optional<double> calcObjectSpeedFromHistory(
     const ObjectData & closest_object, const Path & path, const double current_ego_speed)
   {
+    // in case the object comes from predicted objects info, we reuse the speed.
+    if (closest_object.velocity > 0.0) {
+      this->setPreviousObjectData(closest_object);
+      this->updateVelocityHistory(closest_object.velocity, closest_object.stamp);
+      return this->getMedianObstacleVelocity();
+    }
+
     if (this->checkPreviousObjectDataExpired()) {
       this->setPreviousObjectData(closest_object);
       this->resetVelocityHistory();
@@ -243,6 +254,8 @@ class AEB : public rclcpp::Node
   autoware_universe_utils::InterProcessPollingSubscriber<Imu> sub_imu_{this, "~/input/imu"};
   autoware_universe_utils::InterProcessPollingSubscriber<Trajectory> sub_predicted_traj_{
     this, "~/input/predicted_trajectory"};
+  autoware_universe_utils::InterProcessPollingSubscriber<PredictedObjects> predicted_objects_sub_{
+    this, "~/input/objects"};
   autoware_universe_utils::InterProcessPollingSubscriber<AutowareState> sub_autoware_state_{
     this, "/autoware/state"};
   // publisher
@@ -275,6 +288,10 @@ class AEB : public rclcpp::Node
     std::vector<ObjectData> & objects,
     const pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_ptr);
 
+  void createObjectDataUsingPredictedObjects(
+    const Path & ego_path, const std::vector<Polygon2d> & ego_polys,
+    std::vector<ObjectData> & objects);
+
   void cropPointCloudWithEgoFootprintPath(
     const std::vector<Polygon2d> & ego_polys, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects);
 
@@ -298,6 +315,7 @@ class AEB : public rclcpp::Node
   VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
   Vector3::SharedPtr angular_velocity_ptr_{nullptr};
   Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr};
+  PredictedObjects::ConstSharedPtr predicted_objects_ptr_{nullptr};
   AutowareState::ConstSharedPtr autoware_state_{nullptr};
 
   tf2_ros::Buffer tf_buffer_{get_clock()};
@@ -313,6 +331,8 @@ class AEB : public rclcpp::Node
   bool publish_debug_pointcloud_;
   bool use_predicted_trajectory_;
   bool use_imu_path_;
+  bool use_pointcloud_data_;
+  bool use_predicted_object_data_;
   bool use_object_velocity_calculation_;
   double path_footprint_extra_margin_;
   double detection_range_min_height_;

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

@@ -0,0 +1,88 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_
+#define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_
+
+#include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
+
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <boost/geometry/algorithms/correct.hpp>
+
+#include <tf2/utils.h>
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <vector>
+
+namespace autoware::motion::control::autonomous_emergency_braking::utils
+{
+using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::PredictedObjects;
+using autoware_universe_utils::Point2d;
+using autoware_universe_utils::Polygon2d;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::TransformStamped;
+
+/**
+ * @brief Apply a transform to a predicted object
+ * @param input the predicted object
+ * @param transform_stamped the tf2 transform
+ */
+PredictedObject transformObjectFrame(
+  const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped);
+
+/**
+ * @brief Get the predicted objects polygon as a geometry polygon
+ * @param current_pose the predicted object's pose
+ * @param obj_shape the object's shape
+ */
+Polygon2d convertPolygonObjectToGeometryPolygon(
+  const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape);
+
+/**
+ * @brief Get the predicted objects cylindrical shape as a geometry polygon
+ * @param current_pose the predicted object's pose
+ * @param obj_shape the object's shape
+ */
+Polygon2d convertCylindricalObjectToGeometryPolygon(
+  const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape);
+
+/**
+ * @brief Get the predicted objects bounding box shape as a geometry polygon
+ * @param current_pose the predicted object's pose
+ * @param obj_shape the object's shape
+ */
+Polygon2d convertBoundingBoxObjectToGeometryPolygon(
+  const Pose & current_pose, const double & base_to_front, const double & base_to_rear,
+  const double & base_to_width);
+
+/**
+ * @brief Get the predicted object's shape as a geometry polygon
+ * @param obj the object
+ */
+Polygon2d convertObjToPolygon(const PredictedObject & obj);
+}  // namespace autoware::motion::control::autonomous_emergency_braking::utils
+
+#endif  // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_

control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml

@@ -5,6 +5,7 @@
   <arg name="input_imu" default="/sensing/imu/imu_data"/>
   <arg name="input_odometry" default="/localization/kinematic_state"/>
   <arg name="input_predicted_trajectory" default="/control/trajectory_follower/lateral/predicted_trajectory"/>
+  <arg name="input_objects" default="/perception/object_recognition/objects"/>
 
   <node pkg="autoware_autonomous_emergency_braking" exec="autoware_autonomous_emergency_braking" name="autonomous_emergency_braking" output="screen">
     <!-- load config files -->
@@ -15,5 +16,6 @@
     <remap from="~/input/imu" to="$(var input_imu)"/>
     <remap from="~/input/odometry" to="$(var input_odometry)"/>
     <remap from="~/input/predicted_trajectory" to="$(var input_predicted_trajectory)"/>
+    <remap from="~/input/objects" to="$(var input_objects)"/>
   </node>
 </launch>