diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index df78ffc156802..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 @@ -642,7 +643,7 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) ini_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); const double & dt = imu_prediction_time_interval_; - const double distance_between_points = curr_v * dt; + const double distance_between_points = std::abs(curr_v) * dt; constexpr double minimum_distance_between_points{1e-2}; // if current velocity is too small, assume it stops at the same point // if distance between points is too small, arc length calculation is unreliable, so we skip @@ -887,7 +888,11 @@ void AEB::getClosestObjectsOnPath( 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(); @@ -916,11 +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 bool is_object_in_front_of_ego = obj_arc_length > 0.0; - const double dist_ego_to_object = (is_object_in_front_of_ego) + 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;