Added function to check maximum distance of sensor points

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

Author: SakodaShintaro

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.
+      # 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

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");

localization/ndt_scan_matcher/schema/sub/sensor_points.json

@@ -0,0 +1,20 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Ndt Scan Matcher Node",
+  "definitions": {
+    "frame": {
+      "type": "object",
+      "properties": {
+        "required_distance": {
+          "type": "number",
+          "description": "Required distance of input sensor points. 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
+    }
+  }
+}

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
+                                         << " < " << param_.sensor_points.required_distance);
+    return;
+  }
+
   ndt_ptr_->setInputSource(sensor_points_in_baselink_frame);
   if (!is_activated_) return;