Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

Hi @danielsanchezaran -san,
If you have time, could you look at this this PR and Issue?

• AEB module incorrectly calculates target obstacle's velocity at first calculation #8590

Codecov Report

Attention: Patch coverage is 29.82456% with 40 lines in your changes missing coverage. Please review.

Project coverage is 28.00%. Comparing base (fc283c5) to head (eae0170).
Report is 1 commits behind head on main.

@@            Coverage Diff             @@
##             main    #8591      +/-   ##
==========================================
- Coverage   28.00%   28.00%   -0.01%     
==========================================
  Files        1317     1318       +1     
  Lines       98578    98627      +49     
  Branches    39776    39800      +24     
==========================================
+ Hits        27605    27618      +13     
- Misses      70874    70909      +35     
- Partials       99      100       +1     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

@ismetatabay
Thank you so much for the PR!
The expanded_ego_polys are assigned using the extended footprint only when use_pointcloud_data_ is true, but when use_pointcloud_data_ is false, they remain empty.
Even when empty, they are still used for inside/outside determination in the within function. Is this intended?

@ismetatabay
Shold I stop reviewing? Asking because you reverted to draft.

Hi @kyoichi-sugahara-san, thank you so much for your comments. If you could continue reviewing, I would be grateful. I converted it to a draft as you mentioned a problem with the usage of expanded_ego_polys. I will check this situation but as I mentioned you can continue reviewing, and then I will update it according to your reviews. Thank you so much in advance 🤝

@ismetatabay
Ok thank you for quick replay I will leave some comments,

I can confirm from the description that the results have improved, but do you understand why setting the collision result to false when the object's vertices are outside the extended predicted path helps avoid unnecessary activations?
Also, does this change mean that the speed is being calculated appropriately?

Furthermore, if we were to incorporate this change, I think it would be more natural to implement the process of excluding objects that have vertices not contained within the predicted path in the createObjectDataUsingPointCloudClusters function, rather than checking if it's within the predicted path during collision detection.

The reason is that it's preferable to separate the filtering process for potential collision objects from the actual collision detection.
The current PR seems to be performing the filtering process within the function that executes collision detection, which feels inconsistent with the function name and its processing.
By changing, We can avoid the problem in the comment

I apologize if I've overlooked any circumstances and my comments are unreasonable 🙇

@kyoichi-sugahara -san, thank you for your comments. I plan to refactor my implementation and keep this PR as a draft. My current implementation steps are as follows:

-> Use rough point cloud filtering to keep the velocity information of the closest object in this area.
-> If the closest target object is not located in the predicted path, don't calculate a collision.

As you mentioned, the hasCollision function might not need to filter obstacles based on whether they are in the predicted trajectory. I will refactor it accordingly. Thank you again.

@kyoichi-sugahara -san, my current idea is:

• Add an is_target attribute to the ObjectData struct (default to true for predicted objects).
• Add all objects located in the expanded footprint area (with path_footprint_extra_margin) to the object list (to calculate velocity).
• Check if the object is located in the usual predicted path, then update the object's is_target attribute.
• If the tracked object is not a target (is in the expanded area for velocity calculation but does not enter the predicted path), then don't call the hasCollision function.

What do you think?

Additionally, I found a mistake in the control.launch.xml launch file and I fixed this topic as well.:

@ismetatabay

If the tracked object is not a target (is in the expanded area for velocity calculation but does not enter the predicted path), then don't call the hasCollision function.

So, does this mean that AEB will no longer explicitly react to objects existing outside the normal predicted path?

From referring to the issue (this is super useful result ❤️), I understand that the problem is that the estimation results are unstable for the first few steps after speed calculation begins, causing unnecessary activation.
While reducing the detection targets is a solution, I'm concerned that it might make necessary activations impossible.

From my perspective, I feel the following approaches might be valid solutions:

• Introduce a mechanism that doesn't utilize the first few steps when the speed estimation is clearly unstable.
• Prepare separate areas for speed estimation and for detection targets.

What do you think?

@kyoichi-sugahara -san, thank you for your comment ❤️ .

 So, does this mean that AEB will no longer explicitly react to objects existing outside the normal predicted path? 

AEB maintains the same mechanism for reacting to obstacles in the predicted path. The default current implementation uses ego_polys (predicted path + expand_width) for collision detection and expanded_ego_polys (predicted path + expand_width + path_footprint_extra_margin) for point cloud cropping.

My idea is to calculate the object's velocity when it enters the expanded_ego_polys area. If the object does not enter the ego_polys area (currently used for object detection in the default implementation), we can set the is_target value of this object to false. However, this approach may lead to issues, such as when there are two objects in front of the ego vehicle. If the closer object is within the expanded_ego_polys and the farther one is within the ego_polys, we might not consider the target object (in ego_polys) because it isn't the closest obstacle.

- Introduce a mechanism that doesn't utilize the first few steps when the speed estimation is clearly unstable.

• I think this might cause a slight delay in reaction time, but it could be a quick implementation.

- Prepare separate areas for speed estimation and for detection targets.

• I agree with this idea, and I plan to work on it. This area could be expanded_ego_polys, but we need to implement multiple object tracking mechanism I think. I will check. 👍

@ismetatabay

AEB maintains the same mechanism for reacting to obstacles in the predicted path.

Yes you are right, and sorry I noticed my comment was based on misunderstanding

we might not consider the target object (in ego_polys) because it isn't the closest obstacle.

I understand your concern but, may be the further object can be ignored in my opinion. Only closest object in the predicted path polygon should be considered.

I think this might cause a slight delay in reaction time, but it could be a quick implementation.

yes, that's the concern for this idea which I was discussing with @danielsanchezaran today

I agree with this idea, and I plan to work on it. This area could be expanded_ego_polys, but we need to implement multiple object tracking mechanism I think. I will check. 👍

Thanks! I hope it improves the behavior but still
I feel that it's likely to occur that when a vehicle enters an expanded_ego_polys diagonally, the position of the nearest point doesn't change vertically over time, resulting in a low calculated speed.

@kyoichi-sugahara -san, Thank you so much for your valuable comments and suggestions. My main concern regarding the closest object is that if we calculate and only consider the closest object within separate areas, we might miss the actual target object. I will work on addressing this in the following case:

Since NPC1 is the closest object, we will calculate its speed, but it is not our target object. NPC2 is the target object because it is located on our trajectory. I will check on it.

@ismetatabay it looks like there might be some trouble with using arc coordinates. I cannot really tell why, but detection seems to happen when it should not. I am using the default params.

Hello @danielsanchezaran -san,

I couldn't reproduce the issue you encountered with the planning simulator. If possible, could you share the bag and map files? so I can check the problem

@danielsanchezaran
I have concerns about the ground segmentation performance in your video. Some false positives from ground segmentation in the target area might trigger the emergency. Could you check it?

@danielsanchezaran I have concerns about the ground segmentation performance in your video. Some false positives from ground segmentation in the target area might trigger the emergency. Could you check it?

@ismetatabay thank you for your input. I will try and see if I can reproduce the problem reliably or if it is a tuning problem. The ground segmentation does indeed look suspicious, but I also tried setting the minimum cluster height to 0.1 m to mitigate that, with no luck. Would it be possible to you to do some tests using AWsim? or maybe with a rosbag you have with similar conditions (vehicles close to the ego on both sides)

@ismetatabay I discovered what was the issue.

Some of the MPC path points are virtually the same when the ego is starting to move, that causes the calcLatoffset function to misbehave and not caluclate the proper lateral distance. To solve the issue, we can omit mpc path points that are too close to each other:

--- a/control/autoware_autonomous_emergency_braking/src/node.cpp
+++ b/control/autoware_autonomous_emergency_braking/src/node.cpp
@@ -709,6 +709,9 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj)
for (size_t i = 0; i < predicted_traj.points.size(); ++i) {
geometry_msgs::msg::Pose map_pose;
tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value());

• if (autoware::universe_utils::calcDistance2d(path.back(), map_pose) < 1e-2) {

•  continue;
  

• }
  path.push_back(map_pose);

I would like you to please add this change and also make the minimum distance between points 1e-2 for the IMU path, for consistency.

Also, please note that calcLateralOffset might return NaN, so please handle that case by adding a
if (std::isnan(lateral_offset)) continue;
right after calculating the lat offset on the AEB::getClosestObjectsOnPath function.

As a note, the calcLateralOffset function checks if 2 points are too close, but the epsilon value used (1e-8 might be too small. I hopefully will make a PR next week to fix it).

On the mean time, I will approve this PR, I think it is a great addition to AEB and I am grateful for it, but please fix the issues I pointed out before merging!!!!

@danielsanchezaran Thank you so much for your efforts. Before merging it, I want to test it on a real vehicle as well. I will update you. Have a good weekend!

@danielsanchezaran Thank you so much for your efforts. Before merging it, I want to test it on a real vehicle as well. I will update you. Have a good weekend!

Thank you @ismetatabay any idea when you will you be able to do the tests?