feat(dynamic_obstacle_stop): check the obstacles behind the EGO

[beyzanurkaya]

Description

fixes: #7065

Tests performed

Before the PR:

failed_scenario.mp4

After the PR:

success_scenario.mp4

Notes for reviewers

first, merge this: autowarefoundation/autoware_launch#999

Interface changes

ROS Parameter Changes

Parameter Name	Default Value	Update Description
yaw_threshold	2.35	yaw threshold used for collision checks
yaw_threshold_behind_object	1.0	yaw threshold used for collision checks for objects behind the ego vehicle
ignore_objects_behind_ego	false	If true, ignore objects that are behind the ego vehicle
behind_object_distance_threshold	5.0	distance behind the ego vehicle to ignore objects
use_predicted_path	false	if true, use the predicted path of the object to calculate the collision point

Effects on system behavior

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.

[beyzanurkaya]

@maxime-clem I have completed adding the features but it is not working as I expected. When I use predicted_path for collision check, a collision occurs because this line is empty. I'm investigating this.

autoware.universe/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp

Line 38 in 98cc9f2

boost::geometry::index::intersects(object_footprint), std::back_inserter(rough_collisions));

[maxime-clem]

Thank you for improving this module.
I did not test the PR yet but left a few comments about the code.

[beyzanurkaya]

@maxime-clem Thank you for your feedback. I tested this PR in a few different scenarios and encountered some problems. As a result, I'm thinking of adding a time_to_collison check because the EGO stops even in a situation that comes from behind at low speed and does not threaten the EGO. What do you think?

2024-05-28.18-32-17.mp4

FYI: @brkay54

[beyzanurkaya]

@maxime-clem Some changes I made cause behavior_planner_container to die. I haven't found what the problem is yet. Apart from that, I made the changes you requested, but I did not commit them because I could not solve the node dying problem.

[openscenario_interpreter_node-3] [component_container_mt-32] PC: @                0x0 (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32] *** SIGABRT (@0x3e80024a027) received by PID 2400295 (TID 0x768dd6ffd640) from PID 2400295; stack trace: ***
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768d79eab006 google::(anonymous namespace)::FailureSignalHandler()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06e42520 (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06e969fc pthread_kill
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06e42476 raise
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06e287f3 abort
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072a2b9e (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072ae20c (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072ae277 std::terminate()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072ae4d8 __cxa_throw
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072a27ac (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768d7cc2800d behavior_velocity_planner::dynamic_obstacle_stop::find_closest_collision_point()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768d7cc2866d behavior_velocity_planner::dynamic_obstacle_stop::find_collisions()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768d7cc47e25 behavior_velocity_planner::dynamic_obstacle_stop::DynamicObstacleStopModule::modifyPathVelocity()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf1b676b behavior_velocity_planner::SceneModuleManagerInterface::modifyPathVelocity()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf58a7df autoware::behavior_velocity_planner::BehaviorVelocityPlannerManager::planPathVelocity()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf4a7f08 autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode::generatePath()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf4a868a autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode::onTrigger()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf4e6c07 std::_Function_handler<>::_M_invoke()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf4e2e42 _ZNSt8__detail9__variant17__gen_vtable_implINS0_12_Multi_arrayIPFNS0_21__deduce_visit_resultIvEEOZN6rclcpp23AnySubscriptionCallbackIN27autoware_auto_planning_msgs3msg15PathWithLaneId_ISaIvEEESA_E8dispatchESt10shared_ptrISB_ERKNS5_11MessageInfoEEUlOT_E_RSt7variantIJSt8functionIFvRKSB_EESN_IFvSP_SH_EESN_IFvRKNS5_17SerializedMessageEEESN_IFvSW_SH_EESN_IFvSt10unique_ptrISB_St14default_deleteISB_EEEESN_IFvS14_SH_EESN_IFvS11_ISU_S12_ISU_EEEESN_IFvS1A_SH_EESN_IFvSD_ISO_EEESN_IFvS1F_SH_EESN_IFvSD_ISV_EEESN_IFvS1K_SH_EESN_IFvRKS1F_EESN_IFvS1Q_SH_EESN_IFvRKS1K_EESN_IFvS1W_SH_EESN_IFvSE_EESN_IFvSE_SH_EESN_IFvSD_ISU_EEESN_IFvS25_SH_EEEEEJEEESt16integer_sequenceImJLm8EEEE14__visit_invokeESL_S2B_
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf575d9f rclcpp::Subscription<>::handle_message()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e076227bc rclcpp::Executor::execute_subscription()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e07622fbf rclcpp::Executor::execute_any_executable()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e0762a27a rclcpp::executors::MultiThreadedExecutor::run()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072dc253 (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06e94ac3 (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06f26850 (unknown)

[beyzanurkaya]

I solved the problem of stopping even for slow objects with a simple time_to_collison check.

2024-06-04.10-23-51.mp4