feat(autonomous_emergency_braking): aeb add support negative speeds (a…

…utowarefoundation#7707)

* add support for negative speeds

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

* remove negative speed check for predicted obj

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

---------

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

control/autoware_autonomous_emergency_braking/README.md

@@ -142,7 +142,7 @@ $$
 
 where $yaw_{diff}$ is the difference in yaw between the ego path and the displacement vector $$v_{pos} = o_{pos} - prev_{pos} $$ and $v_{ego}$ is the ego's current speed, which accounts for the movement of points caused by the ego moving and not the object. All these equations are performed disregarding the z axis (in 2D).
 
-Note that, the object velocity is calculated against the ego's current movement direction. If the object moves in the opposite direction to the ego's movement, the object velocity is set to 0 m/s. That is because the RSS distance calculation assumes the ego and the object move in the same direction and it cannot deal with negative velocities.
+Note that, the object velocity is calculated against the ego's current movement direction. If the object moves in the opposite direction to the ego's movement, the object velocity will be negative, which will reduce the rss distance on the next step.
 
 The resulting estimated object speed is added to a queue of speeds with timestamps. The AEB then checks for expiration of past speed estimations and eliminates expired speed measurements from the queue, the object expiration is determined by checking if the time elapsed since the speed was first added to the queue is larger than the parameter `previous_obstacle_keep_time`. Finally, the median speed of the queue is calculated. The median speed will be used to calculate the RSS distance used for collision checking.
 
@@ -151,7 +151,7 @@ The resulting estimated object speed is added to a queue of speeds with timestam
 In the fourth step, it checks the collision with the closest obstacle point using RSS distance. RSS distance is formulated as:
 
 $$
-d = v_{ego}*t_{response} + v_{ego}^2/(2*a_{min}) - v_{obj}^2/(2*a_{obj_{min}}) + offset
+d = v_{ego}*t_{response} + v_{ego}^2/(2*a_{min}) -(sign(v_{obj})) * v_{obj}^2/(2*a_{obj_{min}}) + offset
 $$
 
 where $v_{ego}$ and $v_{obj}$ is current ego and obstacle velocity, $a_{min}$ and $a_{obj_{min}}$ is ego and object minimum acceleration (maximum deceleration), $t_{response}$ is response time of the ego vehicle to start deceleration. Therefore the distance from the ego vehicle to the obstacle is smaller than this RSS distance $d$, the ego vehicle send emergency stop signals. This is illustrated in the following picture.
@@ -221,6 +221,4 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t
 
 - The accuracy of the predicted path created from sensor data depends on the accuracy of sensors attached to the ego vehicle.
 
-- Currently, the module calculates thee closest object velocity if it goes in the same direction as the ego vehicle, otherwise the velocity is set to 0 m/s since RSS distance calculation does not use negative velocities.
-
 ![aeb_range](./image/range.drawio.svg)

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

@@ -181,7 +181,7 @@ class CollisionDataKeeper
     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) {
+    if (std::abs(closest_object.velocity) > std::numeric_limits<double>::epsilon()) {
       this->setPreviousObjectData(closest_object);
       this->updateVelocityHistory(closest_object.velocity, closest_object.stamp);
       return this->getMedianObstacleVelocity();
@@ -218,7 +218,7 @@ class CollisionDataKeeper
         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;
+      return estimated_velocity;
     });
 
     if (!estimated_velocity_opt.has_value()) {

control/autoware_autonomous_emergency_braking/src/node.cpp

@@ -39,6 +39,9 @@
 #include <pcl/segmentation/extract_clusters.h>
 #include <pcl/surface/convex_hull.h>
 #include <tf2/utils.h>
+
+#include <cmath>
+#include <limits>
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -508,9 +511,17 @@ 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 = 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_;
+
+  const double rss_dist = std::invoke([&]() {
+    const double pre_braking_covered_distance = std::abs(current_v) * t;
+    const double braking_distance = (current_v * current_v) / (2 * std::fabs(a_ego_min_));
+    const double ego_stopping_distance = pre_braking_covered_distance + braking_distance;
+    const double obj_braking_distance = (obj_v > 0.0)
+                                          ? -(obj_v * obj_v) / (2 * std::fabs(a_obj_min_))
+                                          : (obj_v * obj_v) / (2 * std::fabs(a_obj_min_));
+    return ego_stopping_distance + obj_braking_distance + longitudinal_offset_;
+  });
+
   if (closest_object.distance_to_object < rss_dist) {
     // collision happens
     ObjectData collision_data = closest_object;
@@ -538,7 +549,7 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w)
     return path;
   }
 
-  constexpr double epsilon = 1e-6;
+  constexpr double epsilon = std::numeric_limits<double>::epsilon();
   const double & dt = imu_prediction_time_interval_;
   const double & horizon = imu_prediction_time_horizon_;
   for (double t = 0.0; t < horizon + epsilon; t += dt) {
@@ -687,7 +698,7 @@ void AEB::createObjectDataUsingPredictedObjects(
         ObjectData obj;
         obj.stamp = stamp;
         obj.position = obj_position;
-        obj.velocity = (obj_tangent_velocity > 0.0) ? obj_tangent_velocity : 0.0;
+        obj.velocity = obj_tangent_velocity;
         obj.distance_to_object = std::abs(dist_ego_to_object);
         object_data_vector.push_back(obj);
         collision_points_added = true;