From b8c9701086e1e7505e87c1adac03c4aafab1b3ff Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 12 Mar 2024 17:56:53 +0900 Subject: [PATCH] feat(ndt_scan_matcher): added a function to check maximum distance of sensor points (#6559) * Added function to check maximum distance of sensor points Signed-off-by: Shintaro SAKODA * style(pre-commit): autofix * Added unit Signed-off-by: Shintaro SAKODA * Fixed json schema Signed-off-by: Shintaro SAKODA --------- Signed-off-by: Shintaro SAKODA Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ndt_scan_matcher/README.md | 4 ++++ .../config/ndt_scan_matcher.param.yaml | 6 ++++++ .../ndt_scan_matcher/hyper_parameters.hpp | 8 ++++++++ .../schema/ndt_scan_matcher.schema.json | 6 +++++- .../schema/sub/sensor_points.json | 18 ++++++++++++++++++ .../src/ndt_scan_matcher_core.cpp | 15 +++++++++++++++ 6 files changed, 56 insertions(+), 1 deletion(-) create mode 100644 localization/ndt_scan_matcher/schema/sub/sensor_points.json diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 74d49e13a4c21..424a7051de883 100644 --- 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 index 144449ce75084..241892e67b66c 100644 --- 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 index 2955e3cb9a5f7..ee1e2369c79bd 100644 --- 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("frame.ndt_base_frame"); frame.map_frame = node->declare_parameter("frame.map_frame"); + sensor_points.required_distance = + node->declare_parameter("sensor_points.required_distance"); + ndt.trans_epsilon = node->declare_parameter("ndt.trans_epsilon"); ndt.step_size = node->declare_parameter("ndt.step_size"); ndt.resolution = node->declare_parameter("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 index a42ee37858f46..74737a098e622 100644 --- 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 new file mode 100644 index 0000000000000..68a0b40f8f94e --- /dev/null +++ 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 index 711dd84e9ea03..d8854757c2f8f 100644 --- 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;