Skip to content

Commit d4460e2

Browse files
YamatoAndopre-commit-ci[bot]
authored and
KhalilSelyan
committed
refactor(ndt scan matcher): update parameter (#7276)
* rename to sensor_points.timeout_sec Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * parameterize skipping_publish_num Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * fix readme Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * parameterize sinitial_to_result_distance_tolerance_m Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * fix build error Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * style(pre-commit): autofix * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent b36b4dc commit d4460e2

File tree

6 files changed

+47
-25
lines changed

6 files changed

+47
-25
lines changed

‎localization/ndt_scan_matcher/README.md

+3-3
Original file line numberDiff line numberDiff line change
@@ -266,7 +266,7 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num
266266
| ------------------------------------------------ | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- |
267267
| `topic_time_stamp` | the time stamp of input topic | none | none | no |
268268
| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes |
269-
| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes |
269+
| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `sensor_points.timeout_sec` | none | yes |
270270
| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes |
271271
| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes |
272272
| `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. |
@@ -280,9 +280,9 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num
280280
| `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 |
281281
| `nearest_voxel_transformation_likelihood_diff` | the nvtl score difference for the current ndt optimization | none | none | no |
282282
| `nearest_voxel_transformation_likelihood_before` | the nvtl score before the current ndt optimization | none | none | no |
283-
| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no |
283+
| `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 |
284284
| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no |
285-
| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - |
285+
| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is `validation.skipping_publish_num` or more | none | - |
286286

287287
※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.
288288

‎localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

+9-3
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,9 @@
1212

1313

1414
sensor_points:
15+
# Tolerance of timestamp difference between current time and sensor pointcloud. [sec]
16+
timeout_sec: 1.0
17+
1518
# Required distance of input sensor points. [m]
1619
# If the max distance of input sensor points is lower than this value, the scan matching will not be performed.
1720
required_distance: 10.0
@@ -52,18 +55,21 @@
5255

5356

5457
validation:
55-
# Tolerance of timestamp difference between current time and sensor pointcloud. [sec]
56-
lidar_topic_timeout_sec: 1.0
57-
5858
# Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]
5959
initial_pose_timeout_sec: 1.0
6060

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

64+
# Tolerance of distance difference from initial pose to result pose. [m]
65+
initial_to_result_distance_tolerance_m: 3.0
66+
6467
# The execution time which means probably NDT cannot matches scans properly. [ms]
6568
critical_upper_bound_exe_time_ms: 100.0
6669

70+
# Tolerance for the number of times rejected estimation results consecutively
71+
skipping_publish_num: 5
72+
6773

6874
score_estimation:
6975
# Converged param type

‎localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp

+8-3
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@ struct HyperParameters
4040

4141
struct SensorPoints
4242
{
43+
double timeout_sec{};
4344
double required_distance{};
4445
} sensor_points{};
4546

@@ -54,10 +55,11 @@ struct HyperParameters
5455

5556
struct Validation
5657
{
57-
double lidar_topic_timeout_sec{};
5858
double initial_pose_timeout_sec{};
5959
double initial_pose_distance_tolerance_m{};
60+
double initial_to_result_distance_tolerance_m{};
6061
double critical_upper_bound_exe_time_ms{};
62+
int64_t skipping_publish_num{};
6163
} validation{};
6264

6365
struct ScoreEstimation
@@ -97,6 +99,7 @@ struct HyperParameters
9799
frame.ndt_base_frame = node->declare_parameter<std::string>("frame.ndt_base_frame");
98100
frame.map_frame = node->declare_parameter<std::string>("frame.map_frame");
99101

102+
sensor_points.timeout_sec = node->declare_parameter<double>("sensor_points.timeout_sec");
100103
sensor_points.required_distance =
101104
node->declare_parameter<double>("sensor_points.required_distance");
102105

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

118-
validation.lidar_topic_timeout_sec =
119-
node->declare_parameter<double>("validation.lidar_topic_timeout_sec");
120121
validation.initial_pose_timeout_sec =
121122
node->declare_parameter<double>("validation.initial_pose_timeout_sec");
122123
validation.initial_pose_distance_tolerance_m =
123124
node->declare_parameter<double>("validation.initial_pose_distance_tolerance_m");
125+
validation.initial_to_result_distance_tolerance_m =
126+
node->declare_parameter<double>("validation.initial_to_result_distance_tolerance_m");
124127
validation.critical_upper_bound_exe_time_ms =
125128
node->declare_parameter<double>("validation.critical_upper_bound_exe_time_ms");
129+
validation.skipping_publish_num =
130+
node->declare_parameter<int64_t>("validation.skipping_publish_num");
126131

127132
const int64_t converged_param_type_tmp =
128133
node->declare_parameter<int64_t>("score_estimation.converged_param_type");

‎localization/ndt_scan_matcher/schema/sub/sensor_points.json

+7-1
Original file line numberDiff line numberDiff line change
@@ -5,13 +5,19 @@
55
"sensor_points": {
66
"type": "object",
77
"properties": {
8+
"timeout_sec": {
9+
"type": "number",
10+
"description": "Tolerance of timestamp difference between current time and sensor pointcloud. [sec]",
11+
"default": 1.0,
12+
"minimum": 0.0
13+
},
814
"required_distance": {
915
"type": "number",
1016
"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.",
1117
"default": "10.0"
1218
}
1319
},
14-
"required": ["required_distance"],
20+
"required": ["timeout_sec", "required_distance"],
1521
"additionalProperties": false
1622
}
1723
}

‎localization/ndt_scan_matcher/schema/sub/validation.json

+15-8
Original file line numberDiff line numberDiff line change
@@ -5,12 +5,6 @@
55
"validation": {
66
"type": "object",
77
"properties": {
8-
"lidar_topic_timeout_sec": {
9-
"type": "number",
10-
"description": "Tolerance of timestamp difference between current time and sensor pointcloud. [sec]",
11-
"default": 1.0,
12-
"minimum": 0.0
13-
},
148
"initial_pose_timeout_sec": {
159
"type": "number",
1610
"description": "Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]",
@@ -23,18 +17,31 @@
2317
"default": 10.0,
2418
"minimum": 0.0
2519
},
20+
"initial_to_result_distance_tolerance_m": {
21+
"type": "number",
22+
"description": "Tolerance of distance difference from initial pose to result pose. [m]",
23+
"default": 3.0,
24+
"minimum": 0.0
25+
},
2626
"critical_upper_bound_exe_time_ms": {
2727
"type": "number",
2828
"description": "The execution time which means probably NDT cannot matches scans properly. [ms]",
2929
"default": 100.0,
3030
"minimum": 0.0
31+
},
32+
"skipping_publish_num": {
33+
"type": "number",
34+
"description": "Tolerance for the number for times rejected estimation results consecutively.",
35+
"default": 5,
36+
"minimum": 1
3137
}
3238
},
3339
"required": [
34-
"lidar_topic_timeout_sec",
3540
"initial_pose_timeout_sec",
3641
"initial_pose_distance_tolerance_m",
37-
"critical_upper_bound_exe_time_ms"
42+
"initial_to_result_distance_tolerance_m",
43+
"critical_upper_bound_exe_time_ms",
44+
"skipping_publish_num"
3845
],
3946
"additionalProperties": false
4047
}

‎localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

+5-7
Original file line numberDiff line numberDiff line change
@@ -300,12 +300,11 @@ void NDTScanMatcher::callback_sensor_points(
300300
callback_sensor_points_main(sensor_points_msg_in_sensor_frame);
301301

302302
// check skipping_publish_num
303-
static size_t skipping_publish_num = 0;
304-
const size_t error_skipping_publish_num = 5;
303+
static int64_t skipping_publish_num = 0;
305304
skipping_publish_num =
306305
((is_succeed_scan_matching || !is_activated_) ? 0 : (skipping_publish_num + 1));
307306
diagnostics_scan_points_->add_key_value("skipping_publish_num", skipping_publish_num);
308-
if (skipping_publish_num >= error_skipping_publish_num) {
307+
if (skipping_publish_num >= param_.validation.skipping_publish_num) {
309308
std::stringstream message;
310309
message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times).";
311310
diagnostics_scan_points_->update_level_and_message(
@@ -340,11 +339,11 @@ bool NDTScanMatcher::callback_sensor_points_main(
340339
(this->now() - sensor_points_msg_in_sensor_frame->header.stamp).seconds();
341340
diagnostics_scan_points_->add_key_value(
342341
"sensor_points_delay_time_sec", sensor_points_delay_time_sec);
343-
if (sensor_points_delay_time_sec > param_.validation.lidar_topic_timeout_sec) {
342+
if (sensor_points_delay_time_sec > param_.sensor_points.timeout_sec) {
344343
std::stringstream message;
345344
message << "sensor points is experiencing latency."
346345
<< "The delay time is " << sensor_points_delay_time_sec << "[sec] "
347-
<< "(the tolerance is " << param_.validation.lidar_topic_timeout_sec << "[sec]).";
346+
<< "(the tolerance is " << param_.sensor_points.timeout_sec << "[sec]).";
348347
diagnostics_scan_points_->update_level_and_message(
349348
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
350349

@@ -578,8 +577,7 @@ bool NDTScanMatcher::callback_sensor_points_main(
578577
const auto distance_initial_to_result = static_cast<double>(
579578
norm(interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position));
580579
diagnostics_scan_points_->add_key_value("distance_initial_to_result", distance_initial_to_result);
581-
const double warn_distance_initial_to_result = 3.0;
582-
if (distance_initial_to_result > warn_distance_initial_to_result) {
580+
if (distance_initial_to_result > param_.validation.initial_to_result_distance_tolerance_m) {
583581
std::stringstream message;
584582
message << "distance_initial_to_result is too large (" << distance_initial_to_result
585583
<< " [m]).";

0 commit comments

Comments
 (0)