diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt
index 4655cf4c2f73a..d56cb3fb31584 100644
--- a/perception/compare_map_segmentation/CMakeLists.txt
+++ b/perception/compare_map_segmentation/CMakeLists.txt
@@ -89,6 +89,8 @@ install(
RUNTIME DESTINATION bin
)
-ament_auto_package(INSTALL_TO_SHARE
+ament_auto_package(
+ INSTALL_TO_SHARE
launch
+ config
)
diff --git a/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml b/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml
new file mode 100644
index 0000000000000..3d5270d17479e
--- /dev/null
+++ b/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml
@@ -0,0 +1,5 @@
+/**:
+ ros__parameters:
+ map_layer_name: "elevation"
+ height_diff_thresh: 0.15
+ map_frame: "map"
diff --git a/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml
new file mode 100644
index 0000000000000..f95c0b0c94b92
--- /dev/null
+++ b/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml
@@ -0,0 +1,8 @@
+/**:
+ ros__parameters:
+ distance_threshold: 0.5
+ use_dynamic_map_loading: true
+ timer_interval_ms: 100
+ map_update_distance_threshold: 10.0
+ map_loader_radius: 150.0
+ publish_debug_pcd: False
diff --git a/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml
new file mode 100644
index 0000000000000..d54f7a96ecda0
--- /dev/null
+++ b/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml
@@ -0,0 +1,9 @@
+/**:
+ ros__parameters:
+ distance_threshold: 0.5
+ use_dynamic_map_loading: true
+ downsize_ratio_z_axis: 0.5
+ timer_interval_ms: 100
+ map_update_distance_threshold: 10.0
+ map_loader_radius: 150.0
+ publish_debug_pcd: False
diff --git a/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml
new file mode 100644
index 0000000000000..d54f7a96ecda0
--- /dev/null
+++ b/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml
@@ -0,0 +1,9 @@
+/**:
+ ros__parameters:
+ distance_threshold: 0.5
+ use_dynamic_map_loading: true
+ downsize_ratio_z_axis: 0.5
+ timer_interval_ms: 100
+ map_update_distance_threshold: 10.0
+ map_loader_radius: 150.0
+ publish_debug_pcd: False
diff --git a/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml
new file mode 100644
index 0000000000000..d54f7a96ecda0
--- /dev/null
+++ b/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml
@@ -0,0 +1,9 @@
+/**:
+ ros__parameters:
+ distance_threshold: 0.5
+ use_dynamic_map_loading: true
+ downsize_ratio_z_axis: 0.5
+ timer_interval_ms: 100
+ map_update_distance_threshold: 10.0
+ map_loader_radius: 150.0
+ publish_debug_pcd: False
diff --git a/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml b/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml
index 42478c514b788..9cb41c8780f13 100644
--- a/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml
+++ b/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml
@@ -1,11 +1,13 @@
+
+
-
+
diff --git a/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml
index 4a272646503f1..cdf17ebfeda9f 100644
--- a/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml
+++ b/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml
@@ -1,11 +1,13 @@
+
+
-
+
diff --git a/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml
index 2942781ce8123..e3f4594729c2b 100644
--- a/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml
+++ b/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml
@@ -1,11 +1,13 @@
+
+
-
+
diff --git a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml
index 2fc2884fd5df9..01ad5cd9b1792 100644
--- a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml
+++ b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml
@@ -1,25 +1,16 @@
+
+
-
-
-
-
-
-
-
-
-
-
-
-
+
diff --git a/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml
index 09ea9022b97e5..13023a8c1c2c9 100644
--- a/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml
+++ b/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml
@@ -1,11 +1,13 @@
+
+
-
+
diff --git a/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json b/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json
new file mode 100644
index 0000000000000..24ac2481a4e4a
--- /dev/null
+++ b/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json
@@ -0,0 +1,43 @@
+{
+ "$schema": "http://json-schema.org/draft-07/schema#",
+ "title": "Parameters for Compare Elevation Map Filter",
+ "type": "object",
+ "definitions": {
+ "compare_elevation_map_filter": {
+ "type": "object",
+ "properties": {
+ "map_layer_name": {
+ "type": "string",
+ "default": "elevation",
+ "description": "elevation map layer name"
+ },
+ "height_diff_thresh": {
+ "type": "number",
+ "default": "0.15",
+ "description": "Remove points whose height difference is below this value [m]"
+ },
+ "map_frame": {
+ "type": "string",
+ "default": "map",
+ "description": "frame_id of the map that is temporarily used before elevation_map is subscribed"
+ }
+ },
+ "required": ["map_layer_name", "height_diff_thresh", "map_frame"],
+ "additionalProperties": false
+ }
+ },
+ "properties": {
+ "/**": {
+ "type": "object",
+ "properties": {
+ "ros__parameters": {
+ "$ref": "#/definitions/compare_elevation_map_filter"
+ }
+ },
+ "required": ["ros__parameters"],
+ "additionalProperties": false
+ }
+ },
+ "required": ["/**"],
+ "additionalProperties": false
+}
diff --git a/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json
new file mode 100644
index 0000000000000..10454b48d3d80
--- /dev/null
+++ b/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json
@@ -0,0 +1,38 @@
+{
+ "$schema": "http://json-schema.org/draft-07/schema#",
+ "title": "Parameters for Distance Based Compare Map Filter",
+ "type": "object",
+ "definitions": {
+ "distance_based_compare_map_filter": {
+ "type": "object",
+ "properties": {
+ "distance_threshold": {
+ "type": "number",
+ "default": "0.5",
+ "description": "Threshold distance to compare input points with map points [m]"
+ },
+ "use_dynamic_map_loading": {
+ "type": "boolean",
+ "default": "true",
+ "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud"
+ }
+ },
+ "required": ["distance_threshold", "use_dynamic_map_loading"],
+ "additionalProperties": false
+ }
+ },
+ "properties": {
+ "/**": {
+ "type": "object",
+ "properties": {
+ "ros__parameters": {
+ "$ref": "#/definitions/distance_based_compare_map_filter"
+ }
+ },
+ "required": ["ros__parameters"],
+ "additionalProperties": false
+ }
+ },
+ "required": ["/**"],
+ "additionalProperties": false
+}
diff --git a/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json
new file mode 100644
index 0000000000000..3c301d5ad9fbb
--- /dev/null
+++ b/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json
@@ -0,0 +1,43 @@
+{
+ "$schema": "http://json-schema.org/draft-07/schema#",
+ "title": "Parameters for Voxel Based Approximate Compare Map Filter",
+ "type": "object",
+ "definitions": {
+ "voxel_based_approximate_compare_map_filter": {
+ "type": "object",
+ "properties": {
+ "distance_threshold": {
+ "type": "number",
+ "default": "0.5",
+ "description": "Threshold distance to compare input points with map points [m]"
+ },
+ "use_dynamic_map_loading": {
+ "type": "boolean",
+ "default": "true",
+ "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud"
+ },
+ "downsize_ratio_z_axis": {
+ "type": "number",
+ "default": "0.5",
+ "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis"
+ }
+ },
+ "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"],
+ "additionalProperties": false
+ }
+ },
+ "properties": {
+ "/**": {
+ "type": "object",
+ "properties": {
+ "ros__parameters": {
+ "$ref": "#/definitions/voxel_based_approximate_compare_map_filter"
+ }
+ },
+ "required": ["ros__parameters"],
+ "additionalProperties": false
+ }
+ },
+ "required": ["/**"],
+ "additionalProperties": false
+}
diff --git a/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json
new file mode 100644
index 0000000000000..2e541662a1743
--- /dev/null
+++ b/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json
@@ -0,0 +1,65 @@
+{
+ "$schema": "http://json-schema.org/draft-07/schema#",
+ "title": "Parameters for Voxel Based Compare Map Filter",
+ "type": "object",
+ "definitions": {
+ "voxel_based_compare_map_filter": {
+ "type": "object",
+ "properties": {
+ "distance_threshold": {
+ "type": "number",
+ "default": "0.5",
+ "description": "Threshold distance to compare input points with map points [m]"
+ },
+ "use_dynamic_map_loading": {
+ "type": "boolean",
+ "default": "true",
+ "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud"
+ },
+ "downsize_ratio_z_axis": {
+ "type": "number",
+ "default": "0.5",
+ "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis"
+ },
+ "map_update_distance_threshold": {
+ "type": "number",
+ "default": "10.0",
+ "description": "Threshold distance to update map points with input points [m]"
+ },
+ "map_loader_radius": {
+ "type": "number",
+ "default": "150.0",
+ "description": "Radius to load map points [m]"
+ },
+ "timer_interval_ms": {
+ "type": "number",
+ "default": "100",
+ "description": "Timer interval to load map points [ms]"
+ }
+ },
+ "required": [
+ "distance_threshold",
+ "use_dynamic_map_loading",
+ "downsize_ratio_z_axis",
+ "map_update_distance_threshold",
+ "map_loader_radius",
+ "timer_interval_ms"
+ ],
+ "additionalProperties": false
+ }
+ },
+ "properties": {
+ "/**": {
+ "type": "object",
+ "properties": {
+ "ros__parameters": {
+ "$ref": "#/definitions/voxel_based_compare_map_filter"
+ }
+ },
+ "required": ["ros__parameters"],
+ "additionalProperties": false
+ }
+ },
+ "required": ["/**"],
+ "additionalProperties": false
+}
diff --git a/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json
new file mode 100644
index 0000000000000..4663dbe806223
--- /dev/null
+++ b/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json
@@ -0,0 +1,43 @@
+{
+ "$schema": "http://json-schema.org/draft-07/schema#",
+ "title": "Parameters for Voxel Distance Based Compare Map Filter",
+ "type": "object",
+ "definitions": {
+ "voxel_distance_based_compare_map_filter": {
+ "type": "object",
+ "properties": {
+ "distance_threshold": {
+ "type": "number",
+ "default": "0.5",
+ "description": "Threshold distance to compare input points with map points [m]"
+ },
+ "use_dynamic_map_loading": {
+ "type": "boolean",
+ "default": "true",
+ "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud"
+ },
+ "downsize_ratio_z_axis": {
+ "type": "number",
+ "default": "0.5",
+ "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis"
+ }
+ },
+ "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"],
+ "additionalProperties": false
+ }
+ },
+ "properties": {
+ "/**": {
+ "type": "object",
+ "properties": {
+ "ros__parameters": {
+ "$ref": "#/definitions/voxel_distance_based_compare_map_filter"
+ }
+ },
+ "required": ["ros__parameters"],
+ "additionalProperties": false
+ }
+ },
+ "required": ["/**"],
+ "additionalProperties": false
+}
diff --git a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp b/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp
index 7c8f2e8d4bd76..1eadeeb3bec05 100644
--- a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp
+++ b/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp
@@ -40,9 +40,9 @@ CompareElevationMapFilterComponent::CompareElevationMapFilterComponent(
: Filter("CompareElevationMapFilter", options)
{
unsubscribe();
- layer_name_ = this->declare_parameter("map_layer_name", std::string("elevation"));
- height_diff_thresh_ = this->declare_parameter("height_diff_thresh", 0.15);
- map_frame_ = this->declare_parameter("map_frame", "map");
+ layer_name_ = declare_parameter("map_layer_name");
+ height_diff_thresh_ = declare_parameter("height_diff_thresh");
+ map_frame_ = declare_parameter("map_frame");
rclcpp::QoS durable_qos{1};
durable_qos.transient_local();