diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 9c5a0f1b73c42..32bedd15c5c53 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -447,8 +447,7 @@ class AEB : public rclcpp::Node */ void getClosestObjectsOnPath( const Path & ego_path, const rclcpp::Time & stamp, - const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects, - const bool ego_moves_forward); + const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects); /** * @brief Create object data using point cloud clusters diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 0087fb70d632c..5d12031f9039b 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -467,9 +468,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) use_pointcloud_data_ && points_belonging_to_cluster_hulls && !points_belonging_to_cluster_hulls->empty()) { const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; - const bool ego_moves_forward = current_v > 0.0; - getClosestObjectsOnPath( - path, current_time, points_belonging_to_cluster_hulls, objects, ego_moves_forward); + getClosestObjectsOnPath(path, current_time, points_belonging_to_cluster_hulls, objects); } if (use_predicted_object_data_) { createObjectDataUsingPredictedObjects(path, ego_polys, objects); @@ -882,15 +881,18 @@ void AEB::getPointsBelongingToClusterHulls( void AEB::getClosestObjectsOnPath( const Path & ego_path, const rclcpp::Time & stamp, - const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects, - const bool ego_moves_forward) + const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // check if the predicted path has a valid number of points if (ego_path.size() < 2 || points_belonging_to_cluster_hulls->empty()) { return; } - + const auto ego_is_driving_forward_opt = autoware::motion_utils::isDrivingForward(ego_path); + if (!ego_is_driving_forward_opt.has_value()) { + return; + } + const bool ego_is_driving_forward = ego_is_driving_forward_opt.value(); // select points inside the ego footprint path const auto current_p = [&]() { const auto & first_point_of_path = ego_path.front(); @@ -919,10 +921,9 @@ void AEB::getClosestObjectsOnPath( // If the object is behind the ego, we need to use the backward long offset. The distance should // be a positive number in any case - const double dist_ego_to_object = (ego_moves_forward) + const double dist_ego_to_object = (ego_is_driving_forward) ? obj_arc_length - vehicle_info_.max_longitudinal_offset_m : obj_arc_length + vehicle_info_.min_longitudinal_offset_m; - ObjectData obj; obj.stamp = stamp; obj.position = obj_position; diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index f2deeed16e033..c2a58941cc144 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -163,8 +163,7 @@ TEST_F(TestAEB, checkImuPathGeneration) aeb_node_->getPointsBelongingToClusterHulls( obstacle_points_ptr, points_belonging_to_cluster_hulls, debug_markers); std::vector objects; - aeb_node_->getClosestObjectsOnPath( - imu_path, stamp, points_belonging_to_cluster_hulls, objects, true); + aeb_node_->getClosestObjectsOnPath(imu_path, stamp, points_belonging_to_cluster_hulls, objects); ASSERT_FALSE(objects.empty()); }