From 4df5f8affb95a0743ca5a2d394c8792473f78efb Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 18 Aug 2023 01:56:27 +0900 Subject: [PATCH] fix(start_planner): fix blinker direction (#4363) * fix(start_planner): fix blinker direction Signed-off-by: kosuke55 * fix Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../start_planner/start_planner_module.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index e4921ba293702..4e27f57b39e7b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -814,10 +814,17 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const if (path.points.empty()) { return {}; } - const auto closest_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - const auto lane_id = path.points.at(closest_idx).lane_ids.front(); - const auto lane = planner_data_->route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id); - const double lateral_offset = lanelet::utils::getLateralDistanceToCenterline(lane, start_pose); + + // calculate lateral offset from pull out target lane center line + lanelet::ConstLanelet closest_road_lane; + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane); + const double lateral_offset = + lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose); if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;