Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(ndt_scan_matcher): added a function to check maximum distance of sensor points #6559

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions localization/ndt_scan_matcher/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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") }}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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": [
Expand All @@ -30,7 +33,8 @@
"validation",
"score_estimation",
"covariance",
"dynamic_map_loading"
"dynamic_map_loading",
"sensor_points"
],
"additionalProperties": false
}
Expand Down
18 changes: 18 additions & 0 deletions localization/ndt_scan_matcher/schema/sub/sensor_points.json
Original file line number Diff line number Diff line change
@@ -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
}
}
}
15 changes: 15 additions & 0 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -346,6 +346,21 @@
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;

Check warning on line 351 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L351

Added line #L351 was not covered by tests
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);

Check warning on line 354 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L354

Added line #L354 was not covered by tests
}
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;

Check warning on line 361 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L361

Added line #L361 was not covered by tests
}

Check warning on line 363 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

NDTScanMatcher::callback_sensor_points increases in cyclomatic complexity from 16 to 18, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
ndt_ptr_->setInputSource(sensor_points_in_baselink_frame);
if (!is_activated_) return;

Expand Down
Loading