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
Changes from 3 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
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
@@ -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");
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": {
"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
}
}
}
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
@@ -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

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

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

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

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;