From 7f368a2f0cc597f515cba3c2ecbbb5ec5f8a6dae Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 29 Apr 2024 13:28:05 +0900 Subject: [PATCH] fix(avoidance): fix bug in the logic to check object moving distance (#6766) fix(avoidance): check object moving distance Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/scene.hpp | 3 -- .../behavior_path_avoidance_module/utils.hpp | 2 - .../src/scene.cpp | 3 -- .../src/utils.cpp | 38 +++++++------------ 4 files changed, 13 insertions(+), 33 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 899ec99cb0d3b..298406c44cf27 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -424,9 +424,6 @@ class AvoidanceModule : public SceneModuleInterface // TODO(Satoshi OTA) create detected object manager. ObjectDataArray registered_objects_; - // TODO(Satoshi OTA) remove mutable. - mutable ObjectDataArray detected_objects_; - // TODO(Satoshi OTA) remove this variable. mutable ObjectDataArray ego_stopped_objects_; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 92e9da61c4ada..d120aab4d6943 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -115,8 +115,6 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters); -void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects); - void updateRegisteredObject( ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, const std::shared_ptr & parameters); diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 1d46e4e1f9d5d..b70b606d8f279 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -410,9 +410,6 @@ ObjectData AvoidanceModule::createObjectData( // Calc moving time. utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_); - // Fill init pose. - utils::avoidance::fillInitialPose(object_data, detected_objects_); - // Calc lateral deviation from path to target object. object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 ? Direction::LEFT diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 33718ae16a038..5ed85dde776a0 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -551,15 +551,6 @@ bool isNeverAvoidanceTarget( const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto is_moving_distance_longer_than_threshold = - tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > - parameters->distance_threshold_for_ambiguous_vehicle; - if (is_moving_distance_longer_than_threshold) { - object.reason = AvoidanceDebugFactor::MOVING_OBJECT; - return true; - } - if (object.is_within_intersection) { if (object.behavior == ObjectData::Behavior::NONE) { object.reason = "ParallelToEgoLane"; @@ -752,6 +743,15 @@ bool isSatisfiedWithVehicleCondition( return false; } + const auto & current_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto is_moving_distance_longer_than_threshold = + calcDistance2d(object.init_pose, current_pose) > + parameters->distance_threshold_for_ambiguous_vehicle; + if (is_moving_distance_longer_than_threshold) { + object.reason = "AmbiguousStoppedVehicle"; + return false; + } + if (object.is_within_intersection) { if (object.behavior == ObjectData::Behavior::DEVIATING) { object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; @@ -1384,6 +1384,7 @@ void fillObjectMovingTime( object_data.last_stop = now; object_data.move_time = 0.0; if (is_new_object) { + object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; object_data.stop_time = 0.0; object_data.last_move = now; stopped_objects.push_back(object_data); @@ -1392,11 +1393,13 @@ void fillObjectMovingTime( same_id_obj->last_stop = now; same_id_obj->move_time = 0.0; object_data.stop_time = same_id_obj->stop_time; + object_data.init_pose = same_id_obj->init_pose; } return; } if (is_new_object) { + object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; object_data.move_time = std::numeric_limits::infinity(); object_data.stop_time = 0.0; object_data.last_move = now; @@ -1406,6 +1409,7 @@ void fillObjectMovingTime( object_data.last_stop = same_id_obj->last_stop; object_data.move_time = (now - same_id_obj->last_stop).seconds(); object_data.stop_time = 0.0; + object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; if (object_data.move_time > object_parameter.moving_time_threshold) { stopped_objects.erase(same_id_obj); @@ -1452,22 +1456,6 @@ void fillAvoidanceNecessity( object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate); } -void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects) -{ - const auto id = object_data.object.object_id; - const auto same_id_obj = std::find_if( - detected_objects.begin(), detected_objects.end(), - [&id](const auto & o) { return o.object.object_id == id; }); - - if (same_id_obj != detected_objects.end()) { - object_data.init_pose = same_id_obj->init_pose; - return; - } - - object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; - detected_objects.push_back(object_data); -} - void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters)