feat(ndt_scan_matcher): added a function to check maximum distance of…

… sensor points (autowarefoundation#6559) * Added function to check maximum distance of sensor points Signed-off-by: Shintaro SAKODA <[email protected]> * style(pre-commit): autofix * Added unit Signed-off-by: Shintaro SAKODA <[email protected]> * Fixed json schema Signed-off-by: Shintaro SAKODA <[email protected]> --------- Signed-off-by: Shintaro SAKODA <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: kaigohirao <[email protected]>
kaigohirao · Mar 22, 2024 · 3559593 · 3559593
1 parent 143f843
commit 3559593
Show file tree

Hide file tree

Showing 6 changed files with 56 additions and 1 deletion.
diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md
@@ -60,6 +60,10 @@ One optional function is regularization. Please see the regularization chapter i
 
 {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/frame.json") }}
 
+#### Sensor Points
+
+{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/sensor_points.json") }}
+
 #### Ndt
 
 {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt.json") }}

diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
@@ -11,6 +11,12 @@
       map_frame: "map"
 
 
+    sensor_points:
+      # Required distance of input sensor points. [m]
+      # If the max distance of input sensor points is lower than this value, the scan matching will not be performed.
+      required_distance: 10.0
+
+
     ndt:
       # The maximum difference between two consecutive
       # transformations in order to consider convergence

diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp
@@ -37,6 +37,11 @@ struct HyperParameters
     std::string map_frame;
   } frame;
 
+  struct SensorPoints
+  {
+    double required_distance;
+  } sensor_points;
+
   pclomp::NdtParams ndt;
   bool ndt_regularization_enable;
 
@@ -91,6 +96,9 @@ struct HyperParameters
     frame.ndt_base_frame = node->declare_parameter<std::string>("frame.ndt_base_frame");
     frame.map_frame = node->declare_parameter<std::string>("frame.map_frame");
 
+    sensor_points.required_distance =
+      node->declare_parameter<double>("sensor_points.required_distance");
+
     ndt.trans_epsilon = node->declare_parameter<double>("ndt.trans_epsilon");
     ndt.step_size = node->declare_parameter<double>("ndt.step_size");
     ndt.resolution = node->declare_parameter<double>("ndt.resolution");

diff --git a/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json b/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json
@@ -21,6 +21,9 @@
             "covariance": { "$ref": "sub/covariance.json#/definitions/covariance" },
             "dynamic_map_loading": {
               "$ref": "sub/dynamic_map_loading.json#/definitions/dynamic_map_loading"
+            },
+            "sensor_points": {
+              "$ref": "sub/sensor_points.json#/definitions/sensor_points"
             }
           },
           "required": [
@@ -30,7 +33,8 @@
             "validation",
             "score_estimation",
             "covariance",
-            "dynamic_map_loading"
+            "dynamic_map_loading",
+            "sensor_points"
           ],
           "additionalProperties": false
         }

diff --git a/localization/ndt_scan_matcher/schema/sub/sensor_points.json b/localization/ndt_scan_matcher/schema/sub/sensor_points.json
@@ -0,0 +1,18 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Ndt Scan Matcher Node",
+  "definitions": {
+    "sensor_points": {
+      "type": "object",
+      "properties": {
+        "required_distance": {
+          "type": "number",
+          "description": "Required distance of input sensor points [m]. If the max distance of input sensor points is lower than this value, the scan matching will not be performed.",
+          "default": "10.0"
+        }
+      },
+      "required": ["required_distance"],
+      "additionalProperties": false
+    }
+  }
+}
diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -346,6 +346,21 @@ void NDTScanMatcher::callback_sensor_points(
   transform_sensor_measurement(
     sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame,
     sensor_points_in_baselink_frame);
+
+  // check max distance of sensor points
+  double max_distance = 0.0;
+  for (const auto & point : sensor_points_in_baselink_frame->points) {
+    const double distance = std::hypot(point.x, point.y, point.z);
+    max_distance = std::max(max_distance, distance);
+  }
+  if (max_distance < param_.sensor_points.required_distance) {
+    RCLCPP_WARN_STREAM(
+      this->get_logger(), "Max distance of sensor points = "
+                            << std::fixed << std::setprecision(3) << max_distance << " [m] < "
+                            << param_.sensor_points.required_distance << " [m]");
+    return;
+  }
+
   ndt_ptr_->setInputSource(sensor_points_in_baselink_frame);
   if (!is_activated_) return;