diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 0ce55c2796f08..1a2004937697a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -189,6 +189,7 @@ class LaneChangeModule : public SceneModuleInterface void updateOutputTurnSignal(BehaviorModuleOutput & output); void updateSteeringFactorPtr(const BehaviorModuleOutput & output); bool isApprovedPathSafe(Pose & ego_pose_before_collision) const; + void calcTurnSignalInfo(); void updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index d9b6f0dad4929..d91a9373bb492 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -132,11 +132,6 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param); -TurnSignalInfo calc_turn_signal_info( - const PathWithLaneId & prepare_path, const double prepare_velocity, - const double min_prepare_distance, const double prepare_duration, const ShiftLine & shift_line, - const ShiftedPath & lane_changing_path); - void get_turn_signal_info( const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index c78da5e8e1769..515435cf5e705 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -697,6 +697,7 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) { + calcTurnSignalInfo(); const auto turn_signal_info = util::getPathTurnSignal( status_.current_lanes, status_.lane_change_path.shifted_path, status_.lane_change_path.shift_line, getEgoPose(), getEgoTwist().linear.x, @@ -706,6 +707,49 @@ void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) lane_change_utils::get_turn_signal_info(status_.lane_change_path, &output.turn_signal_info); } +void LaneChangeModule::calcTurnSignalInfo() +{ + const auto get_blinker_pose = + [this](const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double length) { + const auto & points = path.points; + const auto arc_front = lanelet::utils::getArcCoordinates(lanes, points.front().point.pose); + for (const auto & point : points) { + const auto & pt = point.point.pose; + const auto arc_current = lanelet::utils::getArcCoordinates(lanes, pt); + const auto diff = arc_current.length - arc_front.length; + if (diff > length) { + return pt; + } + } + + RCLCPP_WARN(getLogger(), "unable to determine blinker pose..."); + return points.front().point.pose; + }; + + const auto & path = status_.lane_change_path; + TurnSignalInfo turn_signal_info{}; + + turn_signal_info.desired_start_point = std::invoke([&]() { + const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; + const auto prepare_duration = parameters_->lane_change_prepare_duration; + const auto prepare_to_blinker_start_diff = prepare_duration - blinker_start_duration; + if (prepare_to_blinker_start_diff < 1e-5) { + return path.path.points.front().point.pose; + } + + return get_blinker_pose(path.path, path.reference_lanelets, prepare_to_blinker_start_diff); + }); + turn_signal_info.desired_end_point = path.shift_line.end; + + turn_signal_info.required_start_point = path.shift_line.start; + const auto mid_lane_change_length = path.lane_change_length / 2; + const auto & shifted_path = path.shifted_path.path; + turn_signal_info.required_end_point = + get_blinker_pose(shifted_path, path.target_lanelets, mid_lane_change_length); + + status_.lane_change_path.turn_signal_info = turn_signal_info; +} + void LaneChangeModule::resetParameters() { is_abort_path_approved_ = false; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 1774dddc10d42..fc0305e9b402b 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -99,8 +99,7 @@ std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const double acceleration, const double prepare_duration, const LaneChangePhaseInfo distance, - const LaneChangePhaseInfo speed, const BehaviorPathPlannerParameters & params, + const double acceleration, const LaneChangePhaseInfo distance, const LaneChangePhaseInfo speed, const LaneChangeParameters & lane_change_param) { PathShifter path_shifter; @@ -175,9 +174,6 @@ std::optional constructCandidatePath( return std::nullopt; } - candidate_path.turn_signal_info = lane_change_utils::calc_turn_signal_info( - prepare_segment, speed.prepare, params.minimum_lane_change_prepare_distance, prepare_duration, - shift_line, shifted_path); // check candidate path is in lanelet if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { return std::nullopt; @@ -294,8 +290,7 @@ LaneChangePaths getLaneChangePaths( const auto lc_speed = LaneChangePhaseInfo{prepare_speed, lane_changing_speed}; const auto candidate_path = constructCandidatePath( prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, - shift_line, original_lanelets, target_lanelets, acceleration, lane_change_prepare_duration, - lc_dist, lc_speed, common_parameter, parameter); + shift_line, original_lanelets, target_lanelets, acceleration, lc_dist, lc_speed, parameter); if (!candidate_path) { continue; @@ -710,37 +705,6 @@ bool isEgoWithinOriginalLane( lanelet::utils::to2D(lane_poly).basicPolygon()); } -TurnSignalInfo calc_turn_signal_info( - const PathWithLaneId & prepare_path, const double prepare_velocity, - const double min_prepare_distance, const double prepare_duration, const ShiftLine & shift_line, - const ShiftedPath & lane_changing_path) -{ - TurnSignalInfo turn_signal_info{}; - constexpr double turn_signal_start_duration{3.0}; - turn_signal_info.desired_start_point = - std::invoke([&prepare_path, &prepare_velocity, &min_prepare_distance, &prepare_duration]() { - if (prepare_velocity * turn_signal_start_duration > min_prepare_distance) { - const auto duration = static_cast(prepare_path.points.size()) / prepare_duration; - double time{-duration}; - for (auto itr = prepare_path.points.crbegin(); itr != prepare_path.points.crend(); ++itr) { - time += duration; - if (time >= turn_signal_start_duration) { - return itr->point.pose; - } - } - } - return prepare_path.points.front().point.pose; - }); - - turn_signal_info.required_start_point = shift_line.start; - turn_signal_info.required_end_point = std::invoke([&lane_changing_path]() { - const auto mid_path_idx = lane_changing_path.path.points.size() / 2; - return lane_changing_path.path.points.at(mid_path_idx).point.pose; - }); - turn_signal_info.desired_end_point = shift_line.end; - return turn_signal_info; -} - void get_turn_signal_info( const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info) {