From 12e8d0adee6606becf98fc75e710e59cf6b54391 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 8 Feb 2024 17:53:44 +0900 Subject: [PATCH 1/3] fix behavior_path_planner state fix incorrect comments Signed-off-by: Yuki Takagi --- .../scene.hpp | 2 -- .../src/scene.cpp | 30 ++++++++++--------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp index 696df7f7971ea..bdcd1d7424d37 100644 --- a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp @@ -345,8 +345,6 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - ModuleStatus setInitState() const override { return ModuleStatus::IDLE; } - bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); LatFeasiblePaths generateLateralFeasiblePaths( diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index 73cabd00592a8..85f90bd7d742f 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -1066,28 +1066,30 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( // calculate bound start and end length const double start_length_to_avoid = [&]() { if (obj_vel < parameters_->max_stopped_object_vel) { - // The ego and object are the same directional or the object is parked. + // The ego and object are the opposite directional or the object is parked. return std::min(time_while_collision.time_to_start_collision, 8.0) * std::abs(obj_vel) + std::abs(relative_velocity) * parameters_->start_duration_to_avoid_oncoming_object; + } else { + // The ego and object are the same directional + const double obj_acc = -0.5; + const double decel_time = 1.0; + const double obj_moving_dist = + (std::pow(std::max(obj_vel + obj_acc * decel_time, 0.0), 2) - std::pow(obj_vel, 2)) / 2 / + obj_acc; + const double ego_moving_dist = getEgoSpeed() * decel_time; + return std::max(0.0, ego_moving_dist - obj_moving_dist) + + std::abs(relative_velocity) * parameters_->start_duration_to_avoid_overtaking_object; } - // The ego and object are the opposite directional. - const double obj_acc = -0.5; - const double decel_time = 1.0; - const double obj_moving_dist = - (std::pow(std::max(obj_vel + obj_acc * decel_time, 0.0), 2) - std::pow(obj_vel, 2)) / 2 / - obj_acc; - const double ego_moving_dist = getEgoSpeed() * decel_time; - return std::max(0.0, ego_moving_dist - obj_moving_dist) + - std::abs(relative_velocity) * parameters_->start_duration_to_avoid_overtaking_object; }(); const double end_length_to_avoid = [&]() { if (obj_vel < parameters_->max_stopped_object_vel) { - // The ego and object are the same directional or the object is parked. + // The ego and object are the opposite directional or the object is parked. return std::abs(relative_velocity) * parameters_->end_duration_to_avoid_oncoming_object; + } else { + // The ego and object are the same directional + return std::min(time_while_collision.time_to_end_collision, 3.0) * obj_vel + + std::abs(relative_velocity) * parameters_->end_duration_to_avoid_overtaking_object; } - // The ego and object are the opposite directional. - return std::min(time_while_collision.time_to_end_collision, 3.0) * obj_vel + - std::abs(relative_velocity) * parameters_->end_duration_to_avoid_overtaking_object; }(); // calculate valid path for the forked object's path from the ego's path From f6521dac3001009b532a28d3335f4764f3540822 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 8 Feb 2024 19:33:15 +0900 Subject: [PATCH 2/3] revert Signed-off-by: Yuki Takagi --- .../src/scene.cpp | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index 85f90bd7d742f..8d29bd49e5e28 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -1069,27 +1069,25 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( // The ego and object are the opposite directional or the object is parked. return std::min(time_while_collision.time_to_start_collision, 8.0) * std::abs(obj_vel) + std::abs(relative_velocity) * parameters_->start_duration_to_avoid_oncoming_object; - } else { - // The ego and object are the same directional - const double obj_acc = -0.5; - const double decel_time = 1.0; - const double obj_moving_dist = - (std::pow(std::max(obj_vel + obj_acc * decel_time, 0.0), 2) - std::pow(obj_vel, 2)) / 2 / - obj_acc; - const double ego_moving_dist = getEgoSpeed() * decel_time; - return std::max(0.0, ego_moving_dist - obj_moving_dist) + - std::abs(relative_velocity) * parameters_->start_duration_to_avoid_overtaking_object; } + // The ego and object are the same directional + const double obj_acc = -0.5; + const double decel_time = 1.0; + const double obj_moving_dist = + (std::pow(std::max(obj_vel + obj_acc * decel_time, 0.0), 2) - std::pow(obj_vel, 2)) / 2 / + obj_acc; + const double ego_moving_dist = getEgoSpeed() * decel_time; + return std::max(0.0, ego_moving_dist - obj_moving_dist) + + std::abs(relative_velocity) * parameters_->start_duration_to_avoid_overtaking_object; }(); const double end_length_to_avoid = [&]() { if (obj_vel < parameters_->max_stopped_object_vel) { // The ego and object are the opposite directional or the object is parked. return std::abs(relative_velocity) * parameters_->end_duration_to_avoid_oncoming_object; - } else { - // The ego and object are the same directional - return std::min(time_while_collision.time_to_end_collision, 3.0) * obj_vel + - std::abs(relative_velocity) * parameters_->end_duration_to_avoid_overtaking_object; } + // The ego and object are the same directional + return std::min(time_while_collision.time_to_end_collision, 3.0) * obj_vel + + std::abs(relative_velocity) * parameters_->end_duration_to_avoid_overtaking_object; }(); // calculate valid path for the forked object's path from the ego's path From dcc124d64b56db025b4f071df5f29769f36752ac Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 8 Feb 2024 19:34:47 +0900 Subject: [PATCH 3/3] fix period Signed-off-by: Yuki Takagi --- planning/behavior_path_dynamic_avoidance_module/src/scene.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index 8d29bd49e5e28..7a02f63443178 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -1070,7 +1070,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( return std::min(time_while_collision.time_to_start_collision, 8.0) * std::abs(obj_vel) + std::abs(relative_velocity) * parameters_->start_duration_to_avoid_oncoming_object; } - // The ego and object are the same directional + // The ego and object are the same directional. const double obj_acc = -0.5; const double decel_time = 1.0; const double obj_moving_dist = @@ -1085,7 +1085,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( // The ego and object are the opposite directional or the object is parked. return std::abs(relative_velocity) * parameters_->end_duration_to_avoid_oncoming_object; } - // The ego and object are the same directional + // The ego and object are the same directional. return std::min(time_while_collision.time_to_end_collision, 3.0) * obj_vel + std::abs(relative_velocity) * parameters_->end_duration_to_avoid_overtaking_object; }();