introduce json schema for ground_server

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>
KYabuuchi · Jan 25, 2024 · 33d3e60 · 33d3e60
1 parent 20ff907
commit 33d3e60
Show file tree

Hide file tree

Showing 5 changed files with 59 additions and 12 deletions.
diff --git a/localization/yabloc/yabloc_common/README.md b/localization/yabloc/yabloc_common/README.md
@@ -32,11 +32,7 @@ It estimates the height and tilt of the ground from lanelet2.
 
 ### Parameters
 
-| Name              | Type | Description                                              |
-| ----------------- | ---- | -------------------------------------------------------- |
-| `force_zero_tilt` | bool | if true, the tilt is always determined to be horizontal. |
-| `K`               | int  | parameter for nearest k search                           |
-| `R`               | int  | parameter for radius search                              |
+{{ json_to_markdown("localization/yabloc/yabloc_common/schema/ground_server.schema.json") }}
 
 ## ll2_decomposer
 

diff --git a/localization/yabloc/yabloc_common/config/ground_server.param.yaml b/localization/yabloc/yabloc_common/config/ground_server.param.yaml
@@ -1,5 +1,5 @@
 /**:
   ros__parameters:
     force_zero_tilt: false
-    K: 50
-    R: 10
+    ground_search_neighbors: 50
+    ground_search_radius: 10
diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp
@@ -61,8 +61,8 @@ class GroundServer : public rclcpp::Node
 
 private:
   const bool force_zero_tilt_;
-  const float R;
-  const int K;
+  const float ground_search_radius_;
+  const int ground_search_neighbors_;
 
   // Subscriber
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;

diff --git a/localization/yabloc/yabloc_common/schema/ground_server.schema.json b/localization/yabloc/yabloc_common/schema/ground_server.schema.json
@@ -0,0 +1,51 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for ground_server",
+  "type": "object",
+  "definitions": {
+    "geo_pose_projector": {
+      "type": "object",
+      "properties": {
+        "force_zero_tilt": {
+          "type": "boolean",
+          "description": "if true, the tilt is always determined to be horizontal",
+          "default": false
+        },
+        "ground_search_neighbors": {
+          "type": "number",
+          "description": "the number of neighbors for ground search on a map",
+          "default": "50"
+        },
+        "ground_search_radius": {
+          "type": "number",
+          "description": "radius for ground search on a map [m]",
+          "default": "10"
+        }
+      },
+      "required": [
+        "force_zero_tilt",
+        "ground_search_neighbors",
+        "ground_search_radius"
+      ],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/geo_pose_projector"
+        }
+      },
+      "required": [
+        "ros__parameters"
+      ],
+      "additionalProperties": false
+    }
+  },
+  "required": [
+    "/**"
+  ],
+  "additionalProperties": false
+}
diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp
@@ -33,8 +33,8 @@ namespace yabloc::ground_server
 GroundServer::GroundServer()
 : Node("ground_server"),
   force_zero_tilt_(declare_parameter<bool>("force_zero_tilt")),
-  R(declare_parameter<int>("R")),
-  K(declare_parameter<int>("K"))
+  ground_search_radius_(declare_parameter<int>("ground_search_radius")),
+  ground_search_neighbors_(declare_parameter<int>("ground_search_neighbors"))
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
@@ -191,7 +191,7 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point)
 
   std::vector<int> raw_indices;
   std::vector<float> distances;
-  kdtree_->nearestKSearch(xyz, K, raw_indices, distances);
+  kdtree_->nearestKSearch(xyz, ground_search_neighbors_, raw_indices, distances);
 
   std::vector<int> indices = estimate_inliers_by_ransac(raw_indices);