fix(utils): fix invalid drivable area z-position (autowarefoundation#…

…4866) Signed-off-by: satoshi-ota <[email protected]>
tier4 · Sep 4, 2023 · b2a1b88 · b2a1b88
1 parent d0498e8
commit b2a1b88
Showing 1 changed file with 1 addition and 0 deletions.
diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -1746,6 +1746,7 @@ void makeBoundLongitudinallyMonotonic(
         if (intersect_point) {
           Pose pose;
           pose.position = *intersect_point;
+          pose.position.z = bound_with_pose.at(i).position.z;
           const auto yaw = calcAzimuthAngle(*intersect_point, bound_with_pose.at(i + 1).position);
           pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw);
           monotonic_bound.push_back(pose);