feat(map_loader): add waypoints flag (autowarefoundation#7480)

* feat(map_loader): handle centelrine and waypoints Signed-off-by: Takayuki Murooka <[email protected]> * update README Signed-off-by: Takayuki Murooka <[email protected]> * fix doc Signed-off-by: Takayuki Murooka <[email protected]> * update schema Signed-off-by: Takayuki Murooka <[email protected]> * fix Signed-off-by: Takayuki Murooka <[email protected]> * fix Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
tier4 · Jun 17, 2024 · 564e8c1 · 564e8c1
1 parent 2165e2d
commit 564e8c1
Show file tree

Hide file tree

Showing 6 changed files with 26 additions and 4 deletions.
diff --git a/map/map_loader/README.md b/map/map_loader/README.md
@@ -150,6 +150,9 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie
 
 {{ json_to_markdown("map/map_loader/schema/lanelet2_map_loader.schema.json") }}
 
+`use_waypoints` decides how to handle a centerline.
+This flag enables to use the `overwriteLaneletsCenterlineWithWaypoints` function instead of `overwriteLaneletsCenterline`. Please see [the document of the lanelet2_extension package](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#centerline) in detail.
+
 ---
 
 ## lanelet2_map_visualization

diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml
@@ -1,4 +1,5 @@
 /**:
   ros__parameters:
-    center_line_resolution: 5.0         # [m]
+    center_line_resolution: 5.0                 # [m]
+    use_waypoints: true                         # "centerline" in the Lanelet2 map will be used as a "waypoints" tag.
     lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path
diff --git a/map/map_loader/schema/lanelet2_map_loader.schema.json b/map/map_loader/schema/lanelet2_map_loader.schema.json
@@ -11,13 +11,18 @@
           "description": "Resolution of the Lanelet center line [m]",
           "default": "5.0"
         },
+        "use_waypoints": {
+          "type": "boolean",
+          "description": "If true, `centerline` in the Lanelet2 map will be used as a `waypoints` tag.",
+          "default": true
+        },
         "lanelet2_map_path": {
           "type": "string",
           "description": "The lanelet2 map path pointing to the .osm file",
           "default": ""
         }
       },
-      "required": ["center_line_resolution", "lanelet2_map_path"],
+      "required": ["center_line_resolution", "use_waypoints", "lanelet2_map_path"],
       "additionalProperties": false
     }
   },

diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
@@ -63,13 +63,15 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
 
   declare_parameter<std::string>("lanelet2_map_path");
   declare_parameter<double>("center_line_resolution");
+  declare_parameter<bool>("use_waypoints");
 }
 
 void Lanelet2MapLoaderNode::on_map_projector_info(
   const MapProjectorInfo::Message::ConstSharedPtr msg)
 {
   const auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string();
   const auto center_line_resolution = get_parameter("center_line_resolution").as_double();
+  const auto use_waypoints = get_parameter("use_waypoints").as_bool();
 
   // load map from file
   const auto map = load_map(lanelet2_filename, *msg);
@@ -79,7 +81,11 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
   }
 
   // overwrite centerline
-  lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false);
+  if (use_waypoints) {
+    lanelet::utils::overwriteLaneletsCenterlineWithWaypoints(map, center_line_resolution, false);
+  } else {
+    lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false);
+  }
 
   // create map bin msg
   const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename, now());

diff --git a/map/map_loader/test/lanelet2_map_loader_launch.test.py b/map/map_loader/test/lanelet2_map_loader_launch.test.py
@@ -34,7 +34,13 @@ def generate_test_description():
     lanelet2_map_loader = Node(
         package="map_loader",
         executable="lanelet2_map_loader",
-        parameters=[{"lanelet2_map_path": lanelet2_map_path, "center_line_resolution": 5.0}],
+        parameters=[
+            {
+                "lanelet2_map_path": lanelet2_map_path,
+                "center_line_resolution": 5.0,
+                "use_waypoints": True,
+            }
+        ],
     )
 
     context = {}

diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp
@@ -374,6 +374,7 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_
       Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, *map_projector_info_);
 
     // overwrite more dense centerline
+    // NOTE: overwriteLaneletsCenterlineWithWaypoints is used only in real time calculation.
     lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false);
 
     // create map bin msg