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.

Sign up for GitHub

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

refactor(ndt scan matcher): update parameter #7276

Merged
merged 9 commits into from
Jun 13, 2024
Merged

refactor(ndt scan matcher): update parameter #7276

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
a90a171
rename to sensor_points.timeout_sec
YamatoAndo Jun 5, 2024
0698d3d
parameterize skipping_publish_num
YamatoAndo Jun 5, 2024
403a9f1
fix readme
YamatoAndo Jun 5, 2024
909edc1
parameterize sinitial_to_result_distance_tolerance_m
YamatoAndo Jun 5, 2024
ca3e6da
fix build error
YamatoAndo Jun 5, 2024
eb8dab7
style(pre-commit): autofix
pre-commit-ci[bot] Jun 5, 2024
5da4847
Merge branch 'main' into refactor/ndt_scan_matcher/update_parameter
YamatoAndo Jun 6, 2024
488250e
Merge branch 'main' into refactor/ndt_scan_matcher/update_parameter
YamatoAndo Jun 13, 2024
055e77f
style(pre-commit): autofix
pre-commit-ci[bot] Jun 13, 2024
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
6 changes: 3 additions & 3 deletions localization/ndt_scan_matcher/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -266,7 +266,7 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num
| ------------------------------------------------ | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- |
| `topic_time_stamp` | the time stamp of input topic | none | none | no |
| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes |
| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes |
| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `sensor_points.timeout_sec` | none | yes |
| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes |
| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes |
| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. |
Expand All @@ -280,9 +280,9 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num
| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes |
| `nearest_voxel_transformation_likelihood_diff` | the nvtl score difference for the current ndt optimization | none | none | no |
| `nearest_voxel_transformation_likelihood_before` | the nvtl score before the current ndt optimization | none | none | no |
| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no |
| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than `validation.initial_to_result_distance_tolerance_m` | none | no |
| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no |
| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - |
| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is `validation.skipping_publish_num` or more | none | - |

※The `sensor_points_callback` shares the same callback group as the `trigger_node_service` and `ndt_align_service`. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale.

Expand Down
12 changes: 9 additions & 3 deletions localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,9 @@


sensor_points:
# Tolerance of timestamp difference between current time and sensor pointcloud. [sec]
timeout_sec: 1.0

# 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
Expand Down Expand Up @@ -52,18 +55,21 @@


validation:
# Tolerance of timestamp difference between current time and sensor pointcloud. [sec]
lidar_topic_timeout_sec: 1.0

# Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]
initial_pose_timeout_sec: 1.0

# Tolerance of distance difference between two initial poses used for linear interpolation. [m]
initial_pose_distance_tolerance_m: 10.0

# Tolerance of distance difference from initial pose to result pose. [m]
initial_to_result_distance_tolerance_m: 3.0

# The execution time which means probably NDT cannot matches scans properly. [ms]
critical_upper_bound_exe_time_ms: 100.0

# Tolerance for the number of times rejected estimation results consecutively
skipping_publish_num: 5


score_estimation:
# Converged param type
Expand Down
11 changes: 8 additions & 3 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ struct HyperParameters

struct SensorPoints
{
double timeout_sec{};
double required_distance{};
} sensor_points{};

Expand All @@ -54,10 +55,11 @@ struct HyperParameters

struct Validation
{
double lidar_topic_timeout_sec{};
double initial_pose_timeout_sec{};
double initial_pose_distance_tolerance_m{};
double initial_to_result_distance_tolerance_m{};
double critical_upper_bound_exe_time_ms{};
int64_t skipping_publish_num{};
} validation{};

struct ScoreEstimation
Expand Down Expand Up @@ -97,6 +99,7 @@ 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.timeout_sec = node->declare_parameter<double>("sensor_points.timeout_sec");
sensor_points.required_distance =
node->declare_parameter<double>("sensor_points.required_distance");

Expand All @@ -115,14 +118,16 @@ struct HyperParameters
initial_pose_estimation.n_startup_trials =
node->declare_parameter<int64_t>("initial_pose_estimation.n_startup_trials");

validation.lidar_topic_timeout_sec =
node->declare_parameter<double>("validation.lidar_topic_timeout_sec");
validation.initial_pose_timeout_sec =
node->declare_parameter<double>("validation.initial_pose_timeout_sec");
validation.initial_pose_distance_tolerance_m =
node->declare_parameter<double>("validation.initial_pose_distance_tolerance_m");
validation.initial_to_result_distance_tolerance_m =
node->declare_parameter<double>("validation.initial_to_result_distance_tolerance_m");
validation.critical_upper_bound_exe_time_ms =
node->declare_parameter<double>("validation.critical_upper_bound_exe_time_ms");
validation.skipping_publish_num =
node->declare_parameter<int64_t>("validation.skipping_publish_num");

const int64_t converged_param_type_tmp =
node->declare_parameter<int64_t>("score_estimation.converged_param_type");
Expand Down
8 changes: 7 additions & 1 deletion localization/ndt_scan_matcher/schema/sub/sensor_points.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,19 @@
"sensor_points": {
"type": "object",
"properties": {
"timeout_sec": {
"type": "number",
"description": "Tolerance of timestamp difference between current time and sensor pointcloud. [sec]",
"default": 1.0,
"minimum": 0.0
},
"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"],
"required": ["timeout_sec", "required_distance"],
"additionalProperties": false
}
}
Expand Down
23 changes: 15 additions & 8 deletions localization/ndt_scan_matcher/schema/sub/validation.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,6 @@
"validation": {
"type": "object",
"properties": {
"lidar_topic_timeout_sec": {
"type": "number",
"description": "Tolerance of timestamp difference between current time and sensor pointcloud. [sec]",
"default": 1.0,
"minimum": 0.0
},
"initial_pose_timeout_sec": {
"type": "number",
"description": "Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]",
Expand All @@ -23,18 +17,31 @@
"default": 10.0,
"minimum": 0.0
},
"initial_to_result_distance_tolerance_m": {
"type": "number",
"description": "Tolerance of distance difference from initial pose to result pose. [m]",
"default": 3.0,
"minimum": 0.0
},
"critical_upper_bound_exe_time_ms": {
"type": "number",
"description": "The execution time which means probably NDT cannot matches scans properly. [ms]",
"default": 100.0,
"minimum": 0.0
},
"skipping_publish_num": {
"type": "number",
"description": "Tolerance for the number for times rejected estimation results consecutively.",
"default": 5,
"minimum": 1
}
},
"required": [
"lidar_topic_timeout_sec",
"initial_pose_timeout_sec",
"initial_pose_distance_tolerance_m",
"critical_upper_bound_exe_time_ms"
"initial_to_result_distance_tolerance_m",
"critical_upper_bound_exe_time_ms",
"skipping_publish_num"
],
"additionalProperties": false
}
Expand Down
12 changes: 5 additions & 7 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -300,12 +300,11 @@ void NDTScanMatcher::callback_sensor_points(
callback_sensor_points_main(sensor_points_msg_in_sensor_frame);

// check skipping_publish_num
static size_t skipping_publish_num = 0;
const size_t error_skipping_publish_num = 5;
static int64_t skipping_publish_num = 0;
skipping_publish_num =
((is_succeed_scan_matching || !is_activated_) ? 0 : (skipping_publish_num + 1));
diagnostics_scan_points_->add_key_value("skipping_publish_num", skipping_publish_num);
if (skipping_publish_num >= error_skipping_publish_num) {
if (skipping_publish_num >= param_.validation.skipping_publish_num) {
std::stringstream message;
message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times).";
diagnostics_scan_points_->update_level_and_message(
Expand Down Expand Up @@ -340,11 +339,11 @@ bool NDTScanMatcher::callback_sensor_points_main(
(this->now() - sensor_points_msg_in_sensor_frame->header.stamp).seconds();
diagnostics_scan_points_->add_key_value(
"sensor_points_delay_time_sec", sensor_points_delay_time_sec);
if (sensor_points_delay_time_sec > param_.validation.lidar_topic_timeout_sec) {
if (sensor_points_delay_time_sec > param_.sensor_points.timeout_sec) {
std::stringstream message;
message << "sensor points is experiencing latency."
<< "The delay time is " << sensor_points_delay_time_sec << "[sec] "
<< "(the tolerance is " << param_.validation.lidar_topic_timeout_sec << "[sec]).";
<< "(the tolerance is " << param_.sensor_points.timeout_sec << "[sec]).";
diagnostics_scan_points_->update_level_and_message(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());

Expand Down Expand Up @@ -578,8 +577,7 @@ bool NDTScanMatcher::callback_sensor_points_main(
const auto distance_initial_to_result = static_cast<double>(
norm(interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position));
diagnostics_scan_points_->add_key_value("distance_initial_to_result", distance_initial_to_result);
const double warn_distance_initial_to_result = 3.0;
if (distance_initial_to_result > warn_distance_initial_to_result) {
if (distance_initial_to_result > param_.validation.initial_to_result_distance_tolerance_m) {
std::stringstream message;
message << "distance_initial_to_result is too large (" << distance_initial_to_result
<< " [m]).";
Expand Down
Loading