From 564e8c17e5eab0a667f838b8b2b6ac89b95ff8e6 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 18 Jun 2024 01:25:58 +0900 Subject: [PATCH] feat(map_loader): add waypoints flag (#7480) * feat(map_loader): handle centelrine and waypoints Signed-off-by: Takayuki Murooka * update README Signed-off-by: Takayuki Murooka * fix doc Signed-off-by: Takayuki Murooka * update schema Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- map/map_loader/README.md | 3 +++ map/map_loader/config/lanelet2_map_loader.param.yaml | 3 ++- map/map_loader/schema/lanelet2_map_loader.schema.json | 7 ++++++- .../src/lanelet2_map_loader/lanelet2_map_loader_node.cpp | 8 +++++++- map/map_loader/test/lanelet2_map_loader_launch.test.py | 8 +++++++- .../src/static_centerline_generator_node.cpp | 1 + 6 files changed, 26 insertions(+), 4 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 21bd39303250f..a30bfa1a8f633 100644 --- 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 index b830e038f1de2..48152605ec0a2 100755 --- 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 index fa2b4d363ff92..aae295f847ab2 100644 --- 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 index b097e1809a385..ee89ad571b90a 100644 --- 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,6 +63,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options declare_parameter("lanelet2_map_path"); declare_parameter("center_line_resolution"); + declare_parameter("use_waypoints"); } void Lanelet2MapLoaderNode::on_map_projector_info( @@ -70,6 +71,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( { 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 index f1aa9e0efe922..9f9a59f565e3f 100644 --- 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 index a9c5a667986e7..f5771b9b0b4e8 100644 --- 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