From 9df0ff9141c12cbd96c6c6bcf7d63300da47cc8d Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Wed, 7 Feb 2024 16:27:13 +0700 Subject: [PATCH] chore(map_loader): rework parameters of map_loader (#6199) * Rework parameters of map_loader Signed-off-by: anhnv3991 * style(pre-commit): autofix * Fixed typo in name of map_based_pediction.schema.json, which cause json-schema-check failed Signed-off-by: anhnv3991 * Move path variables back to launch files Signed-off-by: anhnv3991 * Update README.md Signed-off-by: anhnv3991 * Undo the change in perception Signed-off-by: anhnv3991 * Remove default values of declare_parameter from map_loader Signed-off-by: anhnv3991 --------- Signed-off-by: anhnv3991 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- map/map_loader/README.md | 15 +--- .../config/lanelet2_map_loader.param.yaml | 1 + .../config/pointcloud_map_loader.param.yaml | 2 + .../launch/lanelet2_map_loader.launch.xml | 5 +- .../launch/pointcloud_map_loader.launch.xml | 4 +- .../schema/lanelet2_map_loader.schema.json | 38 ++++++++++ .../schema/pointcloud_map_loader.schema.json | 71 +++++++++++++++++++ .../lanelet2_map_loader_node.cpp | 4 +- .../lanelet2_map_visualization_node.cpp | 2 +- .../test/lanelet2_map_loader_launch.test.py | 2 +- 10 files changed, 121 insertions(+), 23 deletions(-) create mode 100644 map/map_loader/schema/lanelet2_map_loader.schema.json create mode 100644 map/map_loader/schema/pointcloud_map_loader.schema.json diff --git a/map/map_loader/README.md b/map/map_loader/README.md index fbe019096a3e7..8a31ecee50be5 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -111,15 +111,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :---------- | :-------------------------------------------------------------------------------- | :------------ | -| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | -| enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | -| enable_partial_load | bool | A flag to enable partial pointcloud map server | false | -| enable_selected_load | bool | A flag to enable selected pointcloud map server | false | -| leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | -| pcd_paths_or_directory | std::string | Path(s) to pointcloud map file or directory | | -| pcd_metadata_path | std::string | Path to pointcloud metadata file | | +{{ json_to_markdown("map/map_loader/schema/pointcloud_map_loader.schema.json") }} ### Interfaces @@ -156,10 +148,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Parameters -| Name | Type | Description | Default value | -| :--------------------- | :---------- | :----------------------------------------------- | :------------ | -| center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | -| lanelet2_map_path | std::string | The lanelet2 map path | None | +{{ json_to_markdown("map/map_loader/schema/lanelet2_map_loader.schema.json") }} --- diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 24d2b0e8ed7a8..b830e038f1de2 100755 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/map_loader/config/lanelet2_map_loader.param.yaml @@ -1,3 +1,4 @@ /**: ros__parameters: center_line_resolution: 5.0 # [m] + lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index b4efbec9706b4..180a3e6f9a2e5 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -7,3 +7,5 @@ # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] + pcd_paths_or_directory: [$(var pcd_paths_or_directory)] # Path to the pointcloud map file or directory + pcd_metadata_path: $(var pcd_metadata_path) # Path to pointcloud metadata file diff --git a/map/map_loader/launch/lanelet2_map_loader.launch.xml b/map/map_loader/launch/lanelet2_map_loader.launch.xml index b24ddae3a53e5..ea0157620ce43 100644 --- a/map/map_loader/launch/lanelet2_map_loader.launch.xml +++ b/map/map_loader/launch/lanelet2_map_loader.launch.xml @@ -5,13 +5,12 @@ - + - - + diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index 9901d04df5645..3e447456bb623 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -7,8 +7,6 @@ - - - + diff --git a/map/map_loader/schema/lanelet2_map_loader.schema.json b/map/map_loader/schema/lanelet2_map_loader.schema.json new file mode 100644 index 0000000000000..fa2b4d363ff92 --- /dev/null +++ b/map/map_loader/schema/lanelet2_map_loader.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for lanelet2 map loader Node", + "type": "object", + "definitions": { + "lanelet2_map_loader": { + "type": "object", + "properties": { + "center_line_resolution": { + "type": "number", + "description": "Resolution of the Lanelet center line [m]", + "default": "5.0" + }, + "lanelet2_map_path": { + "type": "string", + "description": "The lanelet2 map path pointing to the .osm file", + "default": "" + } + }, + "required": ["center_line_resolution", "lanelet2_map_path"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lanelet2_map_loader" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/map_loader/schema/pointcloud_map_loader.schema.json b/map/map_loader/schema/pointcloud_map_loader.schema.json new file mode 100644 index 0000000000000..03cf10b9c5423 --- /dev/null +++ b/map/map_loader/schema/pointcloud_map_loader.schema.json @@ -0,0 +1,71 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for point cloud map loader Node", + "type": "object", + "definitions": { + "pointcloud_map_loader": { + "type": "object", + "properties": { + "enable_whole_load": { + "type": "boolean", + "description": "Enable raw pointcloud map publishing", + "default": true + }, + "enable_downsampled_whole_load": { + "type": "boolean", + "description": "Enable downsampled pointcloud map publishing", + "default": false + }, + "enable_partial_load": { + "type": "boolean", + "description": "Enable partial pointcloud map server", + "default": true + }, + "enable_selected_load": { + "type": "boolean", + "description": "Enable selected pointcloud map server", + "default": false + }, + "leaf_size": { + "type": "number", + "description": "Downsampling leaf size (only used when enable_downsampled_whole_load is set true)", + "default": 3.0 + }, + "pcd_paths_or_directory": { + "type": "array", + "description": "Path(s) to pointcloud map file or directory", + "default": [] + }, + "pcd_metadata_path": { + "type": "string", + "description": "Path to pointcloud metadata file", + "default": "" + } + }, + "required": [ + "enable_whole_load", + "enable_downsampled_whole_load", + "enable_partial_load", + "enable_selected_load", + "leaf_size", + "pcd_paths_or_directory", + "pcd_metadata_path" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pointcloud_map_loader" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "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 03504cc8e2e20..617f3dd503ce0 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 @@ -61,8 +61,8 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); }); - declare_parameter("lanelet2_map_path", ""); - declare_parameter("center_line_resolution", 5.0); + declare_parameter("lanelet2_map_path"); + declare_parameter("center_line_resolution"); } void Lanelet2MapLoaderNode::on_map_projector_info( diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index a0c57759d51a6..870f8d06d90ea 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -71,7 +71,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt { using std::placeholders::_1; - viz_lanelets_centerline_ = this->declare_parameter("viz_lanelets_centerline", true); + viz_lanelets_centerline_ = true; sub_map_bin_ = this->create_subscription( "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), 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 24758a46f75aa..f1aa9e0efe922 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,7 @@ def generate_test_description(): lanelet2_map_loader = Node( package="map_loader", executable="lanelet2_map_loader", - parameters=[{"lanelet2_map_path": lanelet2_map_path}], + parameters=[{"lanelet2_map_path": lanelet2_map_path, "center_line_resolution": 5.0}], ) context = {}