feat(autonomous_emergency_braking): filter and crop aeb pointcloud #6875

danielsanchezaran · 2024-04-24T05:36:06Z

Description

This PR introduces an overhaul to the autonomous_emergency_braking (AEB) module.

Added Euclidean clustering to consider only groups of point clouds that are bigger than a certain size (given by cluster tolerance and min amount of point parameters).
The Point Cloud from sensing is cropped to only include points close to, or inside the ego predicted path.
A Convex hull is created around each cluster of objects. The Vertices of the hull are the points used to calculate if a collision happens or not (the vertices of the hull correspond to the most “external points”, so it really only makes sense to consider said points.
Only the closest Object/Vertex is evaluated for collision, (if ego evaluates that the closest point is safe, the other points will be safe too since they have, by definition, larger distances to the ego).

The goal of these changes is to avoid false positive triggering of the AEB by noisy points, reduce computation time by cropping out unrelated points off of the AEB point cloud, and simplify collision detection by checking the closest point

Example of AEB ignoring objects far from path (AEB points show as red points, normal are grey):

cap-.2024-04-24-14-57-47.mp4

Tests performed

PSIM tests

LSIM
Replayed these rosbags where AEB was wrongly activated, and there was no longer an activation
TIER IV INTERNAL LINK
TIER IV INTERNAL LINK

Notes for reviewers

Requires changes to Launch: autowarefoundation/autoware_launch#966

Interface changes

Effects on system behavior

Reduced false positive detection for AEB

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.
The PR has been properly tested.
The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

There are no open discussions or they are tracked via tickets.
The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

zulfaqar-azmi-t4 · 2024-04-24T08:35:01Z

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -65,6 +67,33 @@ using visualization_msgs::msg::MarkerArray;
 using Path = std::vector<geometry_msgs::msg::Pose>;
 using Vector3 = geometry_msgs::msg::Vector3;

+// For debugging
+class DebugTimer


We have tier4_autoware_utils::StopWatch for this

soblin

Ok, so you are filtering out the obstacle pointclouds that belong to clusters on the (inflated) ego footprint convex hull on the path. And then find the closest point (not the center of the cluster) along ego's path and check safety in the conventional way.

Is it possible to move cropeed_pointcloud_ptr from the member to a local variable / argument ? I prefer having less internal state variables.

danielsanchezaran · 2024-04-24T11:06:36Z

Ok, so you are filtering out the obstacle pointclouds that belong to clusters on the (inflated) ego footprint convex hull on the path. And then find the closest point (not the center of the cluster) along ego's path and check safety in the conventional way.

Is it possible to move cropeed_pointcloud_ptr from the member to a local variable / argument ? I prefer having less internal state variables.

Exactly, I think it is fine to move the point cloud to a local variable, I will push a commit with the fix tomorrow!

tkimura4 · 2024-04-25T03:29:27Z

Could you update Readme ?

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

This reverts commit b5091fc. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

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

danielsanchezaran · 2024-04-25T07:25:42Z

Could you update Readme ?

Done!

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

danielsanchezaran · 2024-04-25T08:12:10Z

Ok, so you are filtering out the obstacle pointclouds that belong to clusters on the (inflated) ego footprint convex hull on the path. And then find the closest point (not the center of the cluster) along ego's path and check safety in the conventional way.

Is it possible to move cropeed_pointcloud_ptr from the member to a local variable / argument ? I prefer having less internal state variables.

Done

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

…utowarefoundation#6875) * enable aeb, fix topic problem Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * eliminate unused var Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * enable aeb, fix topic problem Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * eliminate unused var Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add clustering method to eliminate small objects/noise Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * remove comments Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * Crop points outside of EGO predicted path Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * remove offset Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * Update library use Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add check for empty cloud Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add extra width for pointcloud cropping Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * Use single PC ptr Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * remove problematic option Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * Revert "Use single PC ptr" This reverts commit b5091fc. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * refactoring Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * Add back timestamp Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * consider all points in clusters Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * USe object hull to detect collisions Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * Use only closest object point Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * remove unused functions Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * remove debug timer out of code Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * make it so the clustering uses parameters Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * solve type problem Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * update comment Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * update README Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * eliminate member var in favor of local pointcloud ptr Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * remove unused chrono dependency Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

github-actions bot added the component:control Vehicle control algorithms and mechanisms. (auto-assigned) label Apr 24, 2024

danielsanchezaran marked this pull request as ready for review April 24, 2024 06:57

danielsanchezaran requested review from soblin, TakaHoribe and tkimura4 as code owners April 24, 2024 06:57

danielsanchezaran force-pushed the feat/filter-and-crop-aeb-pointcloud branch from 1a3382e to 74d6251 Compare April 24, 2024 06:59

danielsanchezaran mentioned this pull request Apr 24, 2024

feat(autonomous_emergency_braking): add params for aeb autowarefoundation/autoware_launch#966

Merged

4 tasks

zulfaqar-azmi-t4 reviewed Apr 24, 2024

View reviewed changes

soblin approved these changes Apr 24, 2024

View reviewed changes

danielsanchezaran added 19 commits April 25, 2024 13:42

enable aeb, fix topic problem

7947ea9

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

eliminate unused var

1ae69c0

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

enable aeb, fix topic problem

0e37b9c

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

eliminate unused var

a3920c4

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

add clustering method to eliminate small objects/noise

129734b

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

remove comments

c3da9e8

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

Crop points outside of EGO predicted path

5547e30

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

remove offset

44799b2

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

Update library use

5de93c0

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

add check for empty cloud

061b60c

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

add extra width for pointcloud cropping

29acd56

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

Use single PC ptr

fa7dbc2

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

remove problematic option

22ca648

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

Revert "Use single PC ptr"

7acb276

This reverts commit b5091fc. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

refactoring

e5349cb

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

Add back timestamp

519abf6

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

consider all points in clusters

4b152b1

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

USe object hull to detect collisions

8c7b32e

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

Use only closest object point

ded7be2

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

danielsanchezaran added 5 commits April 25, 2024 13:42

remove unused functions

261d84a

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

remove debug timer out of code

8cd0b69

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

make it so the clustering uses parameters

16e7fb4

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

solve type problem

e4e7c6a

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

update comment

4d6cbeb

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

danielsanchezaran force-pushed the feat/filter-and-crop-aeb-pointcloud branch from 74d6251 to 4d6cbeb Compare April 25, 2024 04:42

update README

bcb9268

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

github-actions bot added the type:documentation Creating or refining documentation. (auto-assigned) label Apr 25, 2024

eliminate member var in favor of local pointcloud ptr

ee9b16b

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

danielsanchezaran added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Apr 25, 2024

danielsanchezaran self-assigned this Apr 25, 2024

remove unused chrono dependency

3aa0c96

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

tkimura4 approved these changes Apr 25, 2024

View reviewed changes

danielsanchezaran merged commit 80eca5e into autowarefoundation:main Apr 26, 2024
20 of 22 checks passed

danielsanchezaran deleted the feat/filter-and-crop-aeb-pointcloud branch April 26, 2024 02:18

danielsanchezaran mentioned this pull request May 17, 2024

feat(aeb): update aeb module and add control evaluator tier4/autoware.universe#1304

Merged

4 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(autonomous_emergency_braking): filter and crop aeb pointcloud #6875

feat(autonomous_emergency_braking): filter and crop aeb pointcloud #6875

danielsanchezaran commented Apr 24, 2024 •

edited by tkimura4

Loading

zulfaqar-azmi-t4 Apr 24, 2024

danielsanchezaran Apr 25, 2024

soblin left a comment

danielsanchezaran commented Apr 24, 2024

tkimura4 commented Apr 25, 2024

danielsanchezaran commented Apr 25, 2024

danielsanchezaran commented Apr 25, 2024

feat(autonomous_emergency_braking): filter and crop aeb pointcloud #6875

feat(autonomous_emergency_braking): filter and crop aeb pointcloud #6875

Conversation

danielsanchezaran commented Apr 24, 2024 • edited by tkimura4 Loading

Description

Related links

Tests performed

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

zulfaqar-azmi-t4 Apr 24, 2024

Choose a reason for hiding this comment

danielsanchezaran Apr 25, 2024

Choose a reason for hiding this comment

soblin left a comment

Choose a reason for hiding this comment

danielsanchezaran commented Apr 24, 2024

tkimura4 commented Apr 25, 2024

danielsanchezaran commented Apr 25, 2024

danielsanchezaran commented Apr 25, 2024

danielsanchezaran commented Apr 24, 2024 •

edited by tkimura4

Loading