fix(obstacle_stop_planner): add checking of point height

[lchojnack]

Description

This PR adds checking of a point height. If a point is lower than the vehicle's height, then the point is in collision with the vehicle. This allows the EGO to drive when an obstacle is above the EGO e.g. tree branches and leaves.

Parameters are added in PR autowarefoundation/autoware_launch#150

Related to:

• issue Vehicle stops in front of non-existing obstacle on the path #2213
• discussion https://github.com/orgs/autowarefoundation/discussions/2981

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.

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.

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

[codecov]

Codecov Report

Base: 11.82% // Head: 11.57% // Decreases project coverage by -0.25% ⚠️

Coverage data is based on head (4d41ceb) compared to base (4c6107a).
Patch coverage: 0.00% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2296      +/-   ##
==========================================
- Coverage   11.82%   11.57%   -0.25%     
==========================================
  Files        1319     1320       +1     
  Lines       92194    92561     +367     
  Branches    24785    24334     -451     
==========================================
- Hits        10900    10714     -186     
- Misses      69934    70665     +731     
+ Partials    11360    11182     -178     

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	11.68% <0.00%> (-0.14%)	⬇️	Carriedforward from 1cbe5c3

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files	Coverage Δ
control/mpc_lateral_controller/src/mpc_utils.cpp	57.27% <ø> (ø)
...lanner/include/obstacle_avoidance_planner/node.hpp	0.00% <ø> (ø)
planning/obstacle_avoidance_planner/src/node.cpp	0.12% <ø> (+<0.01%)	⬆️
...ner/include/obstacle_stop_planner/debug_marker.hpp	0.00% <ø> (ø)
...top_planner/include/obstacle_stop_planner/node.hpp	0.00% <ø> (ø)
...lanning/obstacle_stop_planner/src/debug_marker.cpp	0.00% <0.00%> (ø)
planning/obstacle_stop_planner/src/node.cpp	0.00% <0.00%> (ø)
...anning/obstacle_stop_planner/src/planner_utils.cpp	0.00% <0.00%> (ø)
...nning_simulator/simple_planning_simulator_core.cpp	37.42% <0.00%> (-1.96%)	⬇️
planning/route_handler/src/route_handler.cpp	11.45% <0.00%> (-0.14%)	⬇️
... and 50 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[maxime-clem]

The idea seems good, expect for the case where ego is about to drive up a slope.
I believe in that case we can encounter an obstacle that is currently at a higher z than the vehicle height, but that will still collide with the ego vehicle if it follows its trajectory.

What do you think ?

[lchojnack]

I've updated the PR. I've added a polyhedron for visualization (topic: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker) and colliding point cloud (topic: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/output/point_cloud/debug).

I've tested the PR in some scenarios, and I think it works now. Printscreens below

[maxime-clem]

Thanks for the updates. I think there are some debugging code remaining in the code that need to be removed, otherwise everything looks good.
One thing still missing is an update to the obstacle_stop_planner documentation but this can be updated in a separate PR.
I am now testing the PR with some scenarios to make sure there are no big issue. Once they finish and once the remaining minor code issues are fixed, I will approve the PR.

[lchojnack]

@maxime-clem
I've applied your review hints.
In several days I plan to test the PR with the ITRI simulations, to be sure that the PR resolves the issue #2213

[maxime-clem]

The code looks good and the behavior in simulation is as expected.

[xmfcx]

@lchojnack could you rebase and resolve the conflicts with the planning/obstacle_stop_planner/src/node.cpp please? Then we can proceed to merge it.

[lchojnack]

@xmfcx I've resolved conflicts and I've also tested the implemented functionality. The feature is working as expected. I've tested it using planning simulator.

I think that would be good to test the feature implemented in PR #2647 in order to be sure that during resolving a conflict in planning/obstacle_stop_planner/src/node.cpp the chattering prevention mechanism wasn't broken.

I think the PR requires a review from one of the code owners @satoshi-ota @shmpwk @taikitanaka3 @tkimura4

[xmfcx]

@satoshi-ota could you check if this PR breaks any functionality that was added in your previous PR #2647 ?

[satoshi-ota]

@lchojnack @xmfcx It looks good to me from the point of view of chattering functionality.

[xmfcx]

will resolve the last simple conflicts and merge

[lchojnack]

@xmfcx Thank you for resolving the last simple conflicts.

This is just a friendly reminder. This PR should be merged together with parameters PR autowarefoundation/autoware_launch#150

[brkay54]

Hello @lchojnack @maxime-clem , sorry to disturb you. I am working on z-axis filtering for predicted objects so I want to ask you that how to arrange the z-axis coordinate of the obstacle in planning simulator? I set the value in dummy objects but it is not working 😞

[lchojnack]

Hello @brkay54 ,
I've just checked on the latest autoware on awsim-stable branch hash 984e35c, and the feature works as expected.

Please make sure:

1. This parameter is True
2. There are green box markers on topic
   /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker
   and collision point cloud on topic
   /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/collision_pointcloud as in the picture below

I've used the following command to test
ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/elevation_test/ vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

[brkay54]

Thank you for your answer. If I understood correctly, you used awsim for these tests (setting dummy obstacle). I did not use awsim, just I am using planning simulator.

When I set this value to higher, obstacle position on Z axis did not change.

Also, if you don't mind, can you share your test map with me?

Thank you! @lchojnack

[lchojnack]

@brkay54

If I understood correctly, you used awsim for these tests (setting dummy obstacle).

No, I use planning simulation for testing using the command
ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/elevation_test/ vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

I just have ready to run 'clean' autoware (without any local changes) on awsim-stable branch hash 984e35c.
I use 2D Dummy Bus for testing, and sometimes I modify its H vehicle height as below

Also, if you don't mind, can you share your test map with me?

Please use the map from the tutorial. I've just tested and it also works as expected using command
ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit