From aef30753b343c89e4066f0b81db7f3254abdad00 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 4 Jun 2024 17:32:10 +0900 Subject: [PATCH 1/4] add support for backward path AEB Signed-off-by: Daniel Sanchez --- .../node.hpp | 5 ++++- .../src/node.cpp | 22 ++++++++++++------- 2 files changed, 18 insertions(+), 9 deletions(-) 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 3e72d72cb9946..fb02df1718536 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 @@ -198,7 +198,10 @@ class CollisionDataKeeper const auto nearest_idx = motion_utils::findNearestIndex(path, nearest_collision_point); const auto & nearest_path_pose = path.at(nearest_idx); - const auto & traj_yaw = tf2::getYaw(nearest_path_pose.orientation); + // When the ego moves backwards, the direction of movement axis is reversed + const auto & traj_yaw = (current_ego_speed > 0.0) + ? tf2::getYaw(nearest_path_pose.orientation) + : tf2::getYaw(nearest_path_pose.orientation) + M_PI; const auto estimated_velocity = p_vel * std::cos(p_yaw - traj_yaw) + current_ego_speed; // Current RSS distance calculation does not account for negative velocities diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index d01855bc8ab5d..983e596d94627 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -364,8 +364,9 @@ bool AEB::checkCollision(MarkerArray & debug_markers) } // step2. create velocity data check if the vehicle stops or not + constexpr double min_moving_velocity_th{0.1}; const double current_v = current_velocity_ptr_->longitudinal_velocity; - if (current_v < 0.1) { + if (std::abs(current_v) < min_moving_velocity_th) { return false; } @@ -463,7 +464,8 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object { const double & obj_v = closest_object.velocity; const double & t = t_response_; - const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - + const double rss_dist = std::abs(current_v) * t + + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; if (closest_object.distance_to_object < rss_dist) { // collision happens @@ -487,7 +489,7 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) ini_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); - if (curr_v < 0.1) { + if (std::abs(curr_v) < 0.1) { // if current velocity is too small, assume it stops at the same point return path; } @@ -615,11 +617,15 @@ void AEB::createObjectDataUsingPointCloudClusters( for (const auto & p : *points_belonging_to_cluster_hulls) { const auto obj_position = tier4_autoware_utils::createPoint(p.x, p.y, p.z); - const double dist_ego_to_object = - motion_utils::calcSignedArcLength(ego_path, current_p, obj_position) - - vehicle_info_.max_longitudinal_offset_m; - // objects behind ego are ignored - if (dist_ego_to_object < 0.0) continue; + const double obj_arc_length = + motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); + + // 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) + ? obj_arc_length - vehicle_info_.max_longitudinal_offset_m + : obj_arc_length + vehicle_info_.min_longitudinal_offset_m; ObjectData obj; obj.stamp = stamp; From f10481ef6f5c21aaf4584cc280a4a5911a1299ba Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 5 Jun 2024 18:05:43 +0900 Subject: [PATCH 2/4] fix sign) Signed-off-by: Daniel Sanchez --- .../include/autoware_autonomous_emergency_braking/node.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 fb02df1718536..aca096aac1d82 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 @@ -202,7 +202,8 @@ class CollisionDataKeeper const auto & traj_yaw = (current_ego_speed > 0.0) ? tf2::getYaw(nearest_path_pose.orientation) : tf2::getYaw(nearest_path_pose.orientation) + M_PI; - const auto estimated_velocity = p_vel * std::cos(p_yaw - traj_yaw) + current_ego_speed; + const auto estimated_velocity = + p_vel * std::cos(p_yaw - traj_yaw) + std::abs(current_ego_speed); // Current RSS distance calculation does not account for negative velocities return (estimated_velocity > 0.0) ? estimated_velocity : 0.0; From e7ff83ec65ae373a83292a73bb537258c3b3f41e Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Fri, 7 Jun 2024 15:31:11 +0900 Subject: [PATCH 3/4] add abs and protect against nan Signed-off-by: Daniel Sanchez --- .../autoware_autonomous_emergency_braking/src/node.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 983e596d94627..b26d5eb216d68 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -612,13 +612,17 @@ void AEB::createObjectDataUsingPointCloudClusters( } // select points inside the ego footprint path - const auto current_p = tier4_autoware_utils::createPoint( - ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.z); + const auto current_p = [&]() { + const auto & first_point_of_path = ego_path.front(); + const auto & p = first_point_of_path.position; + return tier4_autoware_utils::createPoint(p.x, p.y, p.z); + }(); for (const auto & p : *points_belonging_to_cluster_hulls) { const auto obj_position = tier4_autoware_utils::createPoint(p.x, p.y, p.z); const double obj_arc_length = motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); + if (std::isnan(obj_arc_length)) continue; // 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 @@ -631,7 +635,7 @@ void AEB::createObjectDataUsingPointCloudClusters( obj.stamp = stamp; obj.position = obj_position; obj.velocity = 0.0; - obj.distance_to_object = dist_ego_to_object; + obj.distance_to_object = std::abs(dist_ego_to_object); const Point2d obj_point(p.x, p.y); for (const auto & ego_poly : ego_polys) { From e1227e4d908f23c1bc57447a29a88df6fe8fcdf3 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 12 Jun 2024 15:06:51 +0900 Subject: [PATCH 4/4] solve sign problem with relative speed Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/src/node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index b26d5eb216d68..1d977b58e9ceb 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -731,7 +731,8 @@ void AEB::addMarker( closest_object_velocity_marker_array.text = "Object velocity: " + std::to_string(obj.velocity) + " [m/s]\n"; closest_object_velocity_marker_array.text += - "Object relative velocity to ego: " + std::to_string(obj.velocity - ego_velocity) + " [m/s]"; + "Object relative velocity to ego: " + std::to_string(obj.velocity - std::abs(ego_velocity)) + + " [m/s]"; debug_markers.markers.push_back(closest_object_velocity_marker_array); } }