From a27c8153a5d596fcdd3b0b8c4a2409474e626efc Mon Sep 17 00:00:00 2001 From: Shintaro SAKODA Date: Thu, 7 Mar 2024 11:11:50 +0900 Subject: [PATCH 1/4] Added function to check maximum distance of sensor points Signed-off-by: Shintaro SAKODA --- .../config/ndt_scan_matcher.param.yaml | 6 ++++++ .../ndt_scan_matcher/hyper_parameters.hpp | 8 ++++++++ .../schema/sub/sensor_points.json | 20 +++++++++++++++++++ .../src/ndt_scan_matcher_core.cpp | 15 ++++++++++++++ 4 files changed, 49 insertions(+) create mode 100644 localization/ndt_scan_matcher/schema/sub/sensor_points.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..a27fcea60987f 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. + # 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/sub/sensor_points.json b/localization/ndt_scan_matcher/schema/sub/sensor_points.json new file mode 100644 index 0000000000000..d3533f3a78256 --- /dev/null +++ b/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 + } + } +} \ No newline at end of file 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 cd637791f04b6..99ed2ad79be60 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 + << " < " << param_.sensor_points.required_distance); + return; + } + ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); if (!is_activated_) return; From bf957b77e64e8c3f0b5419a64333415a13957853 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 7 Mar 2024 06:00:31 +0000 Subject: [PATCH 2/4] style(pre-commit): autofix --- localization/ndt_scan_matcher/schema/sub/sensor_points.json | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/localization/ndt_scan_matcher/schema/sub/sensor_points.json b/localization/ndt_scan_matcher/schema/sub/sensor_points.json index d3533f3a78256..b30775a7c249c 100644 --- a/localization/ndt_scan_matcher/schema/sub/sensor_points.json +++ b/localization/ndt_scan_matcher/schema/sub/sensor_points.json @@ -11,10 +11,8 @@ "default": "10.0" } }, - "required": [ - "required_distance" - ], + "required": ["required_distance"], "additionalProperties": false } } -} \ No newline at end of file +} From 998e11c39c029694422d4f5f499fb39f8d2623e6 Mon Sep 17 00:00:00 2001 From: Shintaro SAKODA Date: Fri, 8 Mar 2024 13:58:30 +0900 Subject: [PATCH 3/4] Added unit Signed-off-by: Shintaro SAKODA --- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 2 +- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) 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 a27fcea60987f..241892e67b66c 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -12,7 +12,7 @@ sensor_points: - # Required distance of input 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 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 99ed2ad79be60..ab444a5b406ef 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -355,9 +355,9 @@ void NDTScanMatcher::callback_sensor_points( } 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); + this->get_logger(), "Max distance of sensor points = " + << std::fixed << std::setprecision(3) << max_distance << " [m] < " + << param_.sensor_points.required_distance << " [m]"); return; } From f0a974ca208208aa435a2288e47b3160de15030b Mon Sep 17 00:00:00 2001 From: Shintaro SAKODA Date: Mon, 11 Mar 2024 11:12:27 +0900 Subject: [PATCH 4/4] Fixed json schema Signed-off-by: Shintaro SAKODA --- localization/ndt_scan_matcher/README.md | 4 ++++ .../ndt_scan_matcher/schema/ndt_scan_matcher.schema.json | 6 +++++- localization/ndt_scan_matcher/schema/sub/sensor_points.json | 4 ++-- 3 files changed, 11 insertions(+), 3 deletions(-) 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/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 index b30775a7c249c..68a0b40f8f94e 100644 --- a/localization/ndt_scan_matcher/schema/sub/sensor_points.json +++ b/localization/ndt_scan_matcher/schema/sub/sensor_points.json @@ -2,12 +2,12 @@ "$schema": "http://json-schema.org/draft-07/schema#", "title": "Parameters for Ndt Scan Matcher Node", "definitions": { - "frame": { + "sensor_points": { "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.", + "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" } },