From 90b9f075b86236dabb9e017e1f4c21cfa94a9fe3 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Date: Mon, 12 Jun 2023 14:52:11 +0300 Subject: [PATCH 1/2] chore(behavior_velocity_speed_bump_module): add maintainer (#3949) Signed-off-by: Mehmet Dogru --- planning/behavior_velocity_speed_bump_module/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_speed_bump_module/package.xml index 2f10b111c043e..3dd16f2fd792a 100644 --- a/planning/behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_speed_bump_module/package.xml @@ -7,6 +7,7 @@ Tomoya Kimura Shumpei Wakabayashi + Mehmet Dogru Apache License 2.0 From 07f8de7d14a88c85de631b5f7786c3e9f8745d45 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 12 Jun 2023 21:26:45 +0900 Subject: [PATCH 2/2] fix(behavior_path_planner): fix offset calculation for turn signal in start_planner (#3945) Signed-off-by: tomoya.kimura --- .../scene_module/start_planner/start_planner_module.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) 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 e9579b8cd9c7a..d478fca57f4b4 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 @@ -796,7 +796,14 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const // pull out path does not overlap const double distance_from_end = motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position); - const double lateral_offset = inverseTransformPoint(end_pose.position, start_pose).y; + + 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); if (distance_from_end < 0.0 && lateral_offset > parameters_->th_blinker_on_lateral_offset) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;