diff --git a/map/util/lanelet2_map_preprocessor/CMakeLists.txt b/map/util/lanelet2_map_preprocessor/CMakeLists.txt index 8a6b16c05011b..982a262455efa 100644 --- a/map/util/lanelet2_map_preprocessor/CMakeLists.txt +++ b/map/util/lanelet2_map_preprocessor/CMakeLists.txt @@ -29,5 +29,6 @@ if(BUILD_TESTING) endif() ament_auto_package(INSTALL_TO_SHARE + config launch ) diff --git a/map/util/lanelet2_map_preprocessor/config/fix_z_value_by_pcd.param.yaml b/map/util/lanelet2_map_preprocessor/config/fix_z_value_by_pcd.param.yaml new file mode 100644 index 0000000000000..de86ef7936d74 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/config/fix_z_value_by_pcd.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + llt_map_path: $(var llt_map_path) + pcd_map_path: $(var pcd_map_path) + llt_output_path: $(var llt_output_path) diff --git a/map/util/lanelet2_map_preprocessor/config/transform_maps.param.yaml b/map/util/lanelet2_map_preprocessor/config/transform_maps.param.yaml new file mode 100644 index 0000000000000..548c28055c834 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/config/transform_maps.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + llt_map_path: $(var llt_map_path) + pcd_map_path: $(var pcd_map_path) + llt_output_path: $(var llt_output_path) + pcd_output_path: $(var pcd_output_path) + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 diff --git a/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml b/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml index 82021ff25d240..5b2aa2e425184 100644 --- a/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml +++ b/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml @@ -1,12 +1,6 @@ - - - - - - - + diff --git a/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml b/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml index 902ca982dace1..950593771878c 100644 --- a/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml +++ b/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml @@ -1,26 +1,6 @@ - - - - - - - - - - - - - - - - - - - - - + diff --git a/map/util/lanelet2_map_preprocessor/schema/transform_maps.schema.json b/map/util/lanelet2_map_preprocessor/schema/transform_maps.schema.json new file mode 100644 index 0000000000000..78fb3952d44ce --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/schema/transform_maps.schema.json @@ -0,0 +1,75 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Transforming Maps", + "type": "object", + "definitions": { + "transform_maps": { + "type": "object", + "properties": { + "llt_map_path": { + "type": "string", + "description": "Path pointing to the input lanelet2 file", + "default": "" + }, + "pcd_map_path": { + "type": "string", + "description": "Path pointing to the input point cloud file", + "default": "" + }, + "llt_output_path": { + "type": "string", + "description": "Path pointing to the output lanelet2 file", + "default": "" + }, + "pcd_output_path": { + "type": "string", + "description": "Path pointing to the output point cloud file", + "default": "" + }, + "x": { + "type": "number", + "default": 0.0, + "description": "x factor of Translation vector for transforming maps [m]" + }, + "y": { + "type": "number", + "default": 0.0, + "description": "y factor of Translation vector for transforming maps [m]" + }, + "z": { + "type": "number", + "default": 0.0, + "description": "z factor of Translation vector for transforming maps [m]" + }, + "roll": { + "type": "number", + "default": 0.0, + "description": "roll factor of Rotation vector for transforming maps [rad]" + }, + "pitch": { + "type": "number", + "default": 0.0, + "description": "pitch factor of Rotation vector for transforming maps [rad]" + }, + "yaw": { + "type": "number", + "default": 0.0, + "description": "yaw factor of Rotation vector for transforming maps [rad]" + } + }, + "required": ["x", "y", "z", "roll", "pitch", "yaw"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/transform_maps" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp index 987f559be1e8e..2317ddb7d9f95 100644 --- a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp +++ b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp @@ -107,12 +107,12 @@ int main(int argc, char * argv[]) const auto pcd_map_path = node->declare_parameter("pcd_map_path"); const auto llt_output_path = node->declare_parameter("llt_output_path"); const auto pcd_output_path = node->declare_parameter("pcd_output_path"); - const auto x = node->declare_parameter("x", 0.0); - const auto y = node->declare_parameter("y", 0.0); - const auto z = node->declare_parameter("z", 0.0); - const auto roll = node->declare_parameter("roll", 0.0); - const auto pitch = node->declare_parameter("pitch", 0.0); - const auto yaw = node->declare_parameter("yaw", 0.0); + const auto x = node->declare_parameter("x"); + const auto y = node->declare_parameter("y"); + const auto z = node->declare_parameter("z"); + const auto roll = node->declare_parameter("roll"); + const auto pitch = node->declare_parameter("pitch"); + const auto yaw = node->declare_parameter("yaw"); std::cout << "transforming maps with following parameters" << std::endl << "x " << x << std::endl