From d9d8692cb9ec76a23af1f9d3abef267a1e946f85 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 19 Jul 2022 17:33:26 +0900 Subject: [PATCH] fix(behavior_velocity_planner): lerp stop point z coordinate (#1370) Signed-off-by: satoshi-ota --- .../src/scene_module/crosswalk/scene_crosswalk.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index 8661c6964fec7..01f479e0d9bec 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -229,11 +229,13 @@ PathPointWithLaneId getBackwardPointFromBasePoint( PathPointWithLaneId output; const double dx = p_to.point.pose.position.x - p_from.point.pose.position.x; const double dy = p_to.point.pose.position.y - p_from.point.pose.position.y; - const double norm = std::hypot(dx, dy); + const double dz = p_to.point.pose.position.z - p_from.point.pose.position.z; + const double norm = std::hypot(dx, dy, dz); output = p_base; output.point.pose.position.x += backward_length * dx / norm; output.point.pose.position.y += backward_length * dy / norm; + output.point.pose.position.z += backward_length * dz / norm; return output; }