From 79b1618b68f1427bf516574caa2222281be47bac Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 19 Mar 2024 18:56:26 +0900 Subject: [PATCH] fix(avoidance): update logic Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/scene.hpp | 50 ------------------- .../src/scene.cpp | 44 +++++++++++++--- 2 files changed, 36 insertions(+), 58 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 e3844f7eb43ea..54f8ccf0b9aea 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 @@ -122,31 +122,6 @@ class AvoidanceModule : public SceneModuleInterface calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position); rtc_interface_ptr_map_.at("left")->updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); - - const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted(); - const auto record_end_time = is_recording_ && !helper_->isShifted(); - const auto steering_factor = [&]() { - if (record_start_time) { - is_recording_ = true; - RCLCPP_DEBUG(getLogger(), "start left avoidance maneuver. get time stamp."); - return uint16_t(100); - } - - if (record_end_time) { - is_recording_ = false; - is_record_necessary_ = false; - RCLCPP_DEBUG(getLogger(), "end avoidance maneuver. get time stamp."); - return uint16_t(200); - } - - return SteeringFactor::LEFT; - }(); - - if (finish_distance > -1.0e-03) { - steering_factor_interface_ptr_->updateSteeringFactor( - {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, - PlanningBehavior::AVOIDANCE, steering_factor, SteeringFactor::TURNING, ""); - } } for (const auto & right_shift : right_shift_array_) { @@ -156,31 +131,6 @@ class AvoidanceModule : public SceneModuleInterface calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position); rtc_interface_ptr_map_.at("right")->updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); - - const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted(); - const auto record_end_time = is_recording_ && !helper_->isShifted(); - const auto steering_factor = [&]() { - if (record_start_time) { - is_recording_ = true; - RCLCPP_DEBUG(getLogger(), "start right avoidance maneuver. get time stamp."); - return uint16_t(100); - } - - if (record_end_time) { - is_recording_ = false; - is_record_necessary_ = false; - RCLCPP_DEBUG(getLogger(), "end avoidance maneuver. get time stamp."); - return uint16_t(200); - } - - return SteeringFactor::RIGHT; - }(); - - if (finish_distance > -1.0e-03) { - steering_factor_interface_ptr_->updateSteeringFactor( - {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, - PlanningBehavior::AVOIDANCE, steering_factor, SteeringFactor::TURNING, ""); - } } } diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 382398338c94c..b45d0bf93273e 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -928,6 +928,42 @@ BehaviorModuleOutput AvoidanceModule::plan() spline_shift_path, data.current_lanelets, planner_data_->parameters.vehicle_info); } + if (!path_shifter_.getShiftLines().empty()) { + const auto front_shift_line = path_shifter_.getShiftLines().front(); + const double start_distance = calcSignedArcLength( + data.reference_path.points, getEgoPosition(), front_shift_line.start.position); + const double finish_distance = calcSignedArcLength( + data.reference_path.points, getEgoPosition(), front_shift_line.end.position); + + const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted(); + const auto record_end_time = is_recording_ && !helper_->isShifted(); + const auto relative_shift_length = front_shift_line.end_shift_length - helper_->getEgoShift(); + const auto steering_direction = + relative_shift_length > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; + const auto steering_status = [&]() { + if (record_start_time) { + is_recording_ = true; + RCLCPP_ERROR(getLogger(), "start right avoidance maneuver. get time stamp."); + return uint16_t(100); + } + + if (record_end_time) { + is_recording_ = false; + is_record_necessary_ = false; + RCLCPP_ERROR(getLogger(), "end avoidance maneuver. get time stamp."); + return uint16_t(200); + } + + return helper_->isShifted() ? SteeringFactor::TURNING : SteeringFactor::APPROACHING; + }(); + + if (finish_distance > -1.0e-03) { + steering_factor_interface_ptr_->updateSteeringFactor( + {front_shift_line.start, front_shift_line.end}, {start_distance, finish_distance}, + PlanningBehavior::AVOIDANCE, steering_direction, steering_status, ""); + } + } + // sparse resampling for computational cost { spline_shift_path.path = utils::resamplePathWithSpline( @@ -1008,14 +1044,6 @@ CandidateOutput AvoidanceModule::planCandidate() const output.start_distance_to_path_change = sl_front.start_longitudinal; output.finish_distance_to_path_change = sl_back.end_longitudinal; - const uint16_t steering_factor_direction = std::invoke([&output]() { - return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; - }); - steering_factor_interface_ptr_->updateSteeringFactor( - {sl_front.start, sl_back.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - PlanningBehavior::AVOIDANCE, steering_factor_direction, SteeringFactor::APPROACHING, ""); - output.path_candidate = shifted_path.path; return output; }