feat(lanelet2_extension): copy the z values in getExpandedLanelet (#141)

* feat(lanelet2_extension): copy the z values in getExpandedLanelet

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Make templated function and add tests

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Fix clang-tidy warning for new gtest

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Fix clang-tidy warnings caused by gtest asserts

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

---------

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp

@@ -1,4 +1,4 @@
-// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+// Copyright 2015-2023 Autoware Foundation. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -24,6 +24,7 @@
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
+#include <lanelet2_core/geometry/Point.h>
 #include <lanelet2_routing/Route.h>
 #include <lanelet2_routing/RoutingGraph.h>
 
@@ -46,6 +47,33 @@ lanelet::ConstLanelet getExpandedLanelet(
 lanelet::ConstLanelets getExpandedLanelets(
   const lanelet::ConstLanelets & lanelet_obj, const double left_offset, const double right_offset);
 
+/// @brief copy the z values between 2 containers based on the 2D arc lengths
+/// @tparam T1 a container of 3D points
+/// @tparam T2 a container of 3D points
+/// @param from points from which the z values will be copied
+/// @param to points to which the z values will be copied
+template <typename T1, typename T2>
+void copyZ(const T1 & from, T2 & to)
+{
+  if (from.empty() || to.empty()) return;
+  to.front().z() = from.front().z();
+  if (from.size() < 2 || to.size() < 2) return;
+  to.back().z() = from.back().z();
+  auto i_from = 1lu;
+  auto s_from = lanelet::geometry::distance2d(from[0], from[1]);
+  auto s_to = 0.0;
+  auto s_from_prev = 0.0;
+  for (auto i_to = 1lu; i_to + 1 < to.size(); ++i_to) {
+    s_to += lanelet::geometry::distance2d(to[i_to - 1], to[i_to]);
+    for (; s_from < s_to && i_from + 1 < from.size(); ++i_from) {
+      s_from_prev = s_from;
+      s_from += lanelet::geometry::distance2d(from[i_from], from[i_from + 1]);
+    }
+    const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev);
+    to[i_to].z() = from[i_from - 1].z() + ratio * (from[i_from].z() - from[i_from - 1].z());
+  }
+}
+
 /**
  * @brief  Apply a patch for centerline because the original implementation
  * doesn't have enough quality

tmp/lanelet2_extension/lib/utilities.cpp

@@ -434,19 +434,18 @@ lanelet::ConstLanelet getExpandedLanelet(
       orig_left_bound_2d.back().y() - left_offset * std::sin(theta);
   }
 
-  const auto toPoints3d = [](const lanelet::BasicLineString2d & ls2d, const double z) {
+  const auto toPoints3d = [](const lanelet::BasicLineString2d & ls2d) {
     lanelet::Points3d output;
     for (const auto & pt : ls2d) {
-      output.push_back(lanelet::Point3d(lanelet::InvalId, pt.x(), pt.y(), z));
+      output.push_back(lanelet::Point3d(lanelet::InvalId, pt.x(), pt.y(), 0.0));
     }
     return output;
   };
 
-  // Original z value cannot be used directly since the offset function can vary the points size.
-  const lanelet::Points3d ex_lefts =
-    toPoints3d(expanded_left_bound_2d, lanelet_obj.leftBound3d().basicLineString().at(0).z());
-  const lanelet::Points3d ex_rights =
-    toPoints3d(expanded_right_bound_2d, lanelet_obj.rightBound3d().basicLineString().at(0).z());
+  lanelet::Points3d ex_lefts = toPoints3d(expanded_left_bound_2d);
+  lanelet::Points3d ex_rights = toPoints3d(expanded_right_bound_2d);
+  copyZ(lanelet_obj.leftBound3d(), ex_lefts);
+  copyZ(lanelet_obj.rightBound3d(), ex_rights);
 
   const auto & extended_left_bound_3d = lanelet::LineString3d(lanelet::InvalId, ex_lefts);
   const auto & expanded_right_bound_3d = lanelet::LineString3d(lanelet::InvalId, ex_rights);

tmp/lanelet2_extension/test/src/test_utilities.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// NOLINTBEGIN(readability-identifier-naming)
+// NOLINTBEGIN(readability-identifier-naming, cppcoreguidelines-avoid-goto)
 
 #include "lanelet2_extension/utility/utilities.hpp"
 
@@ -114,10 +114,51 @@ TEST_F(TestSuite, OverwriteLaneletsCenterline)  // NOLINT for gtest
   }
 }
 
+TEST(Utilities, copyZ)  // NOLINT for gtest
+{
+  using lanelet::utils::copyZ;
+
+  LineString3d ls1;
+  LineString3d ls2;
+
+  ASSERT_NO_THROW(copyZ(ls1, ls2));
+  ls1.push_back(Point3d(lanelet::InvalId, 0.0, 0.0, 5.0));
+  ASSERT_NO_THROW(copyZ(ls1, ls2));
+  ls2.push_back(Point3d(lanelet::InvalId, 0.0, 0.0, 0.0));
+  ASSERT_NO_THROW(copyZ(ls1, ls2));
+  EXPECT_EQ(ls2.front().z(), ls1.front().z());
+  ls1.push_back(Point3d(lanelet::InvalId, 1.0, 0.0, 2.0));
+  ls2.push_back(Point3d(lanelet::InvalId, 3.0, 0.0, 0.0));
+  ASSERT_NO_THROW(copyZ(ls1, ls2));
+  EXPECT_EQ(ls2.front().z(), ls1.front().z());
+  EXPECT_EQ(ls2.back().z(), ls1.back().z());
+
+  ls1.push_back(Point3d(lanelet::InvalId, 2.0, 0.0, 0.0));
+  ls1.push_back(Point3d(lanelet::InvalId, 3.0, 0.0, 3.0));
+  lanelet::Points3d points;
+  points.emplace_back(lanelet::InvalId, 0.0, 0.0, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 0.5, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 1.0, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 1.5, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 2.0, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 2.5, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 3.0, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 3.5, 0.0);
+  ASSERT_NO_THROW(copyZ(ls1, points));
+  EXPECT_EQ(points[0].z(), 5.0);
+  EXPECT_EQ(points[1].z(), 3.5);
+  EXPECT_EQ(points[2].z(), 2.0);
+  EXPECT_EQ(points[3].z(), 1.0);
+  EXPECT_EQ(points[4].z(), 0.0);
+  EXPECT_EQ(points[5].z(), 1.5);
+  EXPECT_EQ(points[6].z(), 3.0);
+  EXPECT_EQ(points[7].z(), 3.0);
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }
 
-// NOLINTEND(readability-identifier-naming)
+// NOLINTEND(readability-identifier-naming, cppcoreguidelines-avoid-goto)