fix(static_drivable_area_expansion): fix invalid z-pos (autowarefoundation#6357)

satoshi-ota · satoshi-ota · commit 0bd89545e1a3 · 2024-02-23T09:32:58.000+09:00
Signed-off-by: satoshi-ota &lt;satoshi.ota928@gmail.com&gt;
diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
@@ -1395,7 +1395,7 @@ std::pair<std::vector<lanelet::ConstPoint3d>, bool> getBoundWithFreeSpaceAreas(
 
       if (intersect.has_value()) {
         ret.emplace_back(
-          lanelet::InvalId, intersect.value().x, intersect.value().y, intersect.value().z);
+          lanelet::InvalId, intersect.value().x, intersect.value().y, toGeomMsgPt(bound.at(i)).z);
         break;
       }
     }

Original file line number	Diff line number	Diff line change
`@@ -1395,7 +1395,7 @@ std::pair<std::vector<lanelet::ConstPoint3d>, bool> getBoundWithFreeSpaceAreas(`
`1395`	`1395`
`1396`	`1396`	`if (intersect.has_value()) {`
`1397`	`1397`	`ret.emplace_back(`
`1398`		`- lanelet::InvalId, intersect.value().x, intersect.value().y, intersect.value().z);`
	`1398`	`+ lanelet::InvalId, intersect.value().x, intersect.value().y, toGeomMsgPt(bound.at(i)).z);`
`1399`	`1399`	`break;`
`1400`	`1400`	`}`
`1401`	`1401`	`}`