feat(lanelet2_extension): centerline is converted to waypoints

Signed-off-by: Takayuki Murooka <[email protected]>

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

@@ -56,7 +56,7 @@ lanelet::ConstLanelets getExpandedLanelets(
  * doesn't have enough quality
  */
 void overwriteLaneletsCenterline(
- lanelet::LaneletMapPtr lanelet_map, const double resolution = 5.0,
+ lanelet::LaneletMapPtr lanelet_map, const double resolution, const bool use_waypoints,
  const bool force_overwrite = false);
 
 lanelet::ConstLanelets getConflictingLanelets(

tmp/lanelet2_extension/lib/utilities.cpp

@@ -501,12 +501,28 @@ lanelet::ConstLanelets getExpandedLanelets(
 }
 
 void overwriteLaneletsCenterline(
- lanelet::LaneletMapPtr lanelet_map, const double resolution, const bool force_overwrite)
+ lanelet::LaneletMapPtr lanelet_map, const double resolution, const bool use_waypoints,
+ const bool force_overwrite)
 {
  for (auto & lanelet_obj : lanelet_map->laneletLayer) {
- if (force_overwrite || !lanelet_obj.hasCustomCenterline()) {
+ if (force_overwrite) {
  const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution);
  lanelet_obj.setCenterline(fine_center_line);
+ } else {
+ if (use_waypoints) {
+ if (lanelet_obj.hasCustomCenterline()) {
+ const auto & centerline = lanelet_obj.centerline();
+ lanelet_obj.setAttribute("waypoints", centerline.id());
+ }
+
+ const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution);
+ lanelet_obj.setCenterline(fine_center_line);
+ } else {
+ if (!lanelet_obj.hasCustomCenterline()) {
+ const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution);
+ lanelet_obj.setCenterline(fine_center_line);
+ }
+ }
  }
  }
 }

tmp/lanelet2_extension/test/src/test_utilities.cpp

@@ -87,6 +87,14 @@ class TestSuite : public ::testing::Test // NOLINT for gtest
  merging_lanelet.attributes()[lanelet::AttributeName::Subtype] =
  lanelet::AttributeValueString::Road;
 
+ // create sample lanelets
+ Point3d cp1 = Point3d(getId(), 0.5, 0., 0.);
+ Point3d cp2 = Point3d(getId(), 0.5, 0.5, 0.);
+ Point3d cp3 = Point3d(getId(), 0.5, 1., 0.);
+
+ LineString3d ls_centerline(getId(), {cp1, cp2, cp3});
+ road_lanelet.setCenterline(ls_centerline);
+
  sample_map_ptr->add(road_lanelet);
  sample_map_ptr->add(next_lanelet);
  sample_map_ptr->add(next_lanelet2);
@@ -104,12 +112,50 @@ class TestSuite : public ::testing::Test // NOLINT for gtest
 private:
 };
 
-TEST_F(TestSuite, OverwriteLaneletsCenterline) // NOLINT for gtest
+TEST_F(TestSuite, OverwriteLaneletsCenterlineWithWaypoints) // NOLINT for gtest
+{
+ double resolution = 5.0;
+ bool force_overwrite = false;
+ bool use_waypoints = true;
+
+ // memorize the original information of the centerline
+ std::unordered_map<lanelet::Id, lanelet::Id> lanelet_centerline_map{};
+ for (const auto & lanelet : sample_map_ptr->laneletLayer) {
+ if (lanelet.hasCustomCenterline()) {
+ lanelet_centerline_map.insert({lanelet.id(), lanelet.centerline().id()});
+ }
+ }
+
+ // convert centerline to waypoints
+ lanelet::utils::overwriteLaneletsCenterline(
+ sample_map_ptr, resolution, use_waypoints, force_overwrite);
+
+ for (const auto & lanelet : sample_map_ptr->laneletLayer) {
+ if (lanelet_centerline_map.find(lanelet.id()) != lanelet_centerline_map.end()) {
+ // check if the lanelet has waypoints.
+ ASSERT_TRUE(lanelet.hasAttribute("waypoints")) << "The lanelet does not have a waypoints.";
+ // check if the waypoints points to the linestring of the centerline.
+ ASSERT_TRUE(
+ lanelet.attribute("waypoints").asId().value() == lanelet_centerline_map.at(lanelet.id()))
+ << "The waypoint's ID is invalid.";
+ }
+ }
+
+ // check if all the lanelets have a centerline
+ for (const auto & lanelet : sample_map_ptr->laneletLayer) {
+ ASSERT_TRUE(lanelet.hasCustomCenterline()) << "failed to calculate fine centerline";
+ }
+}
+
+TEST_F(TestSuite, OverwriteLaneletsCenterlineWithoutWaypoints) // NOLINT for gtest
 {
  double resolution = 5.0;
  bool force_overwrite = false;
- lanelet::utils::overwriteLaneletsCenterline(sample_map_ptr, resolution, force_overwrite);
+ bool use_waypoints = false;
+ lanelet::utils::overwriteLaneletsCenterline(
+ sample_map_ptr, resolution, use_waypoints, force_overwrite);
 
+ // check if all the lanelets have a centerline
  for (const auto & lanelet : sample_map_ptr->laneletLayer) {
  ASSERT_TRUE(lanelet.hasCustomCenterline()) << "failed to calculate fine centerline";
  }