Skip to content

Commit

Permalink
refactor(ndt scan matcher): update parameter (#7276)
Browse files Browse the repository at this point in the history 
* rename to sensor_points.timeout_sec

Signed-off-by: Yamato Ando <[email protected]>

* parameterize skipping_publish_num

Signed-off-by: Yamato Ando <[email protected]>

* fix readme

Signed-off-by: Yamato Ando <[email protected]>

* parameterize sinitial_to_result_distance_tolerance_m

Signed-off-by: Yamato Ando <[email protected]>

* fix build error

Signed-off-by: Yamato Ando <[email protected]>

* style(pre-commit): autofix

* style(pre-commit): autofix

---------

Signed-off-by: Yamato Ando <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@YamatoAndo @pre-commit-ci
YamatoAndo and pre-commit-ci[bot] authored Jun 13, 2024
1 parent f25396f commit d17bfa1
Show file tree
Hide file tree
Showing 6 changed files with 47 additions and 25 deletions.
6 changes: 3 additions & 3 deletions localization/ndt_scan_matcher/README.md
View file
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
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
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
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
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
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

0 comments on commit d17bfa1

Please sign in to comment.