refactor(compare_map_segmentation): rework parameters (#5005)

* refactor the configuration files of the node compare_map_segmentation according to the new ROS node config guideline.

Signed-off-by: yuntianyi-chen <yuntianyichen@outlook.com>

* style(pre-commit): autofix

* Add three parameters

Signed-off-by: yuntianyi-chen <yuntianyichen@outlook.com>

* style(pre-commit): autofix

* Update perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml

Co-authored-by: badai nguyen  <94814556+badai-nguyen@users.noreply.github.com>

* Update perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml

Co-authored-by: badai nguyen  <94814556+badai-nguyen@users.noreply.github.com>

* Update perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml

Co-authored-by: badai nguyen  <94814556+badai-nguyen@users.noreply.github.com>

* Update perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml

Co-authored-by: badai nguyen  <94814556+badai-nguyen@users.noreply.github.com>

* Update perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json

Co-authored-by: badai nguyen  <94814556+badai-nguyen@users.noreply.github.com>

---------

Signed-off-by: yuntianyi-chen <yuntianyichen@outlook.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com>

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
 )

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"

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

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

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

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

perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml

@@ -1,11 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
 <launch>
+  <arg name="compare_elevation_map_filter_param_file" default="$(find-pkg-share compare_map_segmentation)/config/compare_elevation_map_filter.param.yaml"/>
   <arg name="input" default="/input" description="input topic name"/>
   <arg name="input_elevation_map" default="/input/elevation_map" description="input elevation_map topic name"/>
   <arg name="output" default="/output" description="output topic name"/>
-
   <node pkg="compare_map_segmentation" exec="compare_elevation_map_filter_node" name="compare_elevation_map_filter_node" output="screen">
     <remap from="input" to="$(var input)"/>
     <remap from="input/elevation_map" to="$(var input_elevation_map)"/>
     <remap from="output" to="$(var output)"/>
+    <param from="$(var compare_elevation_map_filter_param_file)"/>
   </node>
 </launch>

perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml

@@ -1,11 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
 <launch>
+  <arg name="distance_based_compare_map_filter_param_file" default="$(find-pkg-share compare_map_segmentation)/config/distance_based_compare_map_filter.param.yaml"/>
   <arg name="input" default="/input" description="input topic name"/>
   <arg name="input_map" default="/map" description="input map topic name"/>
   <arg name="output" default="/output" description="output topic name"/>
-
   <node pkg="compare_map_segmentation" exec="distance_based_compare_map_filter_node" name="distance_based_compare_map_filter_node" output="screen">
     <remap from="input" to="$(var input)"/>
     <remap from="map" to="$(var input_map)"/>
     <remap from="output" to="$(var output)"/>
+    <param from="$(var distance_based_compare_map_filter_param_file)"/>
   </node>
 </launch>

perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml

@@ -1,11 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
 <launch>
+  <arg name="voxel_based_approximate_compare_map_filter_param_file" default="$(find-pkg-share compare_map_segmentation)/config/voxel_based_approximate_compare_map_filter.param.yaml"/>
   <arg name="input" default="/input" description="input topic name"/>
   <arg name="input_map" default="/map" description="input map topic name"/>
   <arg name="output" default="/output" description="output topic name"/>
-
   <node pkg="compare_map_segmentation" exec="voxel_based_approximate_compare_map_filter_node" name="voxel_based_approximate_compare_map_filter_node" output="screen">
     <remap from="input" to="$(var input)"/>
     <remap from="map" to="$(var input_map)"/>
     <remap from="output" to="$(var output)"/>
+    <param from="$(var voxel_based_approximate_compare_map_filter_param_file)"/>
   </node>
 </launch>

perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml

@@ -1,25 +1,16 @@
+<?xml version="1.0" encoding="UTF-8"?>
 <launch>
+  <arg name="voxel_based_compare_map_filter_param_file" default="$(find-pkg-share compare_map_segmentation)/config/voxel_based_compare_map_filter.param.yaml"/>
   <arg name="input" default="/input" description="input topic name"/>
   <arg name="input_map" default="/map" description="input map topic name"/>
   <arg name="pose_with_covariance" default="/localization/pose_estimator/pose_with_covariance" description="input vehicle pose topic name"/>
   <arg name="output" default="/output" description="output topic name"/>
   <arg name="map_loader_service" default="/map/get_differential_pointcloud_map" description="map loader service name"/>
-  <arg name="distance_threshold" default="0.3"/>
-  <arg name="downsize_ratio_z_axis" default="0.5"/>
-  <arg name="use_dynamic_map_loading" default="true"/>
-  <arg name="timer_interval_ms" default="100"/>
-  <arg name="map_update_distance_threshold" default="10.0"/>
-  <arg name="map_loader_radius" default="150.0"/>
 
   <node pkg="compare_map_segmentation" exec="voxel_based_compare_map_filter_node" name="voxel_based_compare_map_filter_node" output="screen">
     <remap from="input" to="$(var input)"/>
     <remap from="map" to="$(var input_map)"/>
     <remap from="output" to="$(var output)"/>
-    <param name="distance_threshold" value="$(var distance_threshold)"/>
-    <param name="downsize_ratio_z_axis" value="$(var downsize_ratio_z_axis)"/>
-    <param name="use_dynamic_map_loading" value="$(var use_dynamic_map_loading)"/>
-    <param name="timer_interval_ms" value="$(var timer_interval_ms)"/>
-    <param name="map_update_distance_threshold" value="$(var map_update_distance_threshold)"/>
-    <param name="map_loader_radius" value="$(var map_loader_radius)"/>
+    <param from="$(var voxel_based_compare_map_filter_param_file)"/>
   </node>
 </launch>

perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml

@@ -1,11 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
 <launch>
+  <arg name="voxel_distance_based_compare_map_filter_param_file" default="$(find-pkg-share compare_map_segmentation)/config/voxel_distance_based_compare_map_filter.param.yaml"/>
   <arg name="input" default="/input" description="input topic name"/>
   <arg name="input_map" default="/map" description="input map topic name"/>
   <arg name="output" default="/output" description="output topic name"/>
-
   <node pkg="compare_map_segmentation" exec="voxel_distance_based_compare_map_filter_node" name="voxel_distance_based_compare_map_filter_node" output="screen">
     <remap from="input" to="$(var input)"/>
     <remap from="map" to="$(var input_map)"/>
     <remap from="output" to="$(var output)"/>
+    <param from="$(var voxel_distance_based_compare_map_filter_param_file)"/>
   </node>
 </launch>

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
+}

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
+}

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
+}

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
+}

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
+}

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<std::string>("map_layer_name");
+  height_diff_thresh_ = declare_parameter<float>("height_diff_thresh");
+  map_frame_ = declare_parameter<std::string>("map_frame");
 
   rclcpp::QoS durable_qos{1};
   durable_qos.transient_local();