From 8a9965a2ed06a6e8fe868c7866b17945d475c6f5 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Mon, 18 Mar 2024 16:27:35 +0900 Subject: [PATCH] feat(lane_change_module): add general method to get turn signal for LC module (#6627) add general method to get turn signal for LC module Signed-off-by: Daniel Sanchez --- .../scene.hpp | 2 - .../utils/base_class.hpp | 2 - .../src/scene.cpp | 75 ++++--------------- 3 files changed, 14 insertions(+), 65 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 454282d4eacd3..3b8d2ad6e9931 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -151,8 +151,6 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - TurnSignalInfo calcTurnSignalInfo() const override; - bool isValidPath(const PathWithLaneId & path) const override; PathSafetyStatus isLaneChangePathSafe( diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 3bb250c5affd8..3efc0c9dd61dc 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -218,8 +218,6 @@ class LaneChangeBase LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const = 0; - virtual TurnSignalInfo calcTurnSignalInfo() const = 0; - virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index d7cd2b209943f..03569aa4f3cdd 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -480,14 +480,20 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { - TurnSignalInfo turn_signal_info = calcTurnSignalInfo(); - const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal( - status_.current_lanes, status_.lane_change_path.shifted_path, - status_.lane_change_path.info.shift_line, getEgoPose(), getEgoTwist().linear.x, - planner_data_->parameters); - turn_signal_info.turn_signal.command = turn_signal_command.command; - - return turn_signal_info; + const auto & pose = getEgoPose(); + const auto & current_lanes = status_.current_lanes; + const auto & shift_line = status_.lane_change_path.info.shift_line; + const auto & shift_path = status_.lane_change_path.shifted_path; + const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; + constexpr bool is_driving_forward = true; + // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's + // current lane, lane change is different, so we set this flag to false. + constexpr bool egos_lane_is_shifted = false; + + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward, + egos_lane_is_shifted); + return new_signal; } lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const @@ -1687,59 +1693,6 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( return {}; } -TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const -{ - const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) { - double accumulated_length = 0.0; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - accumulated_length += - tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (accumulated_length > length) { - return path.points.at(i).point.pose; - } - } - - return path.points.front().point.pose; - }; - - const auto & path = status_.lane_change_path; - const auto & shifted_path = path.shifted_path.path; - - TurnSignalInfo turn_signal_info{}; - - if (path.path.points.empty()) { - return turn_signal_info; - } - - // desired start pose = prepare start pose - turn_signal_info.desired_start_point = std::invoke([&]() { - const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; - const auto diff_time = path.info.duration.prepare - blinker_start_duration; - if (diff_time < 1e-5) { - return path.path.points.front().point.pose; - } - - const auto current_twist = getEgoTwist(); - const auto diff_length = std::abs(current_twist.linear.x) * diff_time; - return get_blinker_pose(path.path, diff_length); - }); - - // desired end pose - const auto length_ratio = - std::clamp(lane_change_parameters_->length_ratio_for_turn_signal_deactivation, 0.0, 1.0); - const auto desired_end_length = path.info.length.lane_changing * length_ratio; - turn_signal_info.desired_end_point = get_blinker_pose(shifted_path, desired_end_length); - - // required start pose = lane changing start pose - turn_signal_info.required_start_point = path.info.shift_line.start; - - // required end pose = in the middle of the lane change - const auto mid_lane_change_length = path.info.length.lane_changing / 2; - turn_signal_info.required_end_point = get_blinker_pose(shifted_path, mid_lane_change_length); - - return turn_signal_info; -} - bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const { const auto & route_handler = planner_data_->route_handler;