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): remake diag #5076

Merged
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
91 commits
Select commit Hold shift + click to select a range
5d76260
feat(ndt_scan_matcher): remake diag
Sep 22, 2023
c7d880c
style(pre-commit): autofix
pre-commit-ci[bot] Sep 22, 2023
80ffb52
add latest_ndt_aling_service_best_score
Sep 22, 2023
0f2543f
style(pre-commit): autofix
pre-commit-ci[bot] Sep 22, 2023
a227961
check nullptr
Sep 25, 2023
923368b
style(pre-commit): autofix
pre-commit-ci[bot] Sep 25, 2023
219eaa8
add validate_distance_from_initial_to_result
Sep 25, 2023
b3089ad
style(pre-commit): autofix
pre-commit-ci[bot] Sep 25, 2023
dc40889
rename
Sep 25, 2023
e2f18ad
style(pre-commit): autofix
pre-commit-ci[bot] Sep 25, 2023
42c805e
Merge branch 'main' into feat/remake_ndt_scan_matcher_diag
YamatoAndo Apr 8, 2024
744ff3f
style(pre-commit): autofix
pre-commit-ci[bot] Apr 8, 2024
6d0ef62
[WIP] update
YamatoAndo Apr 11, 2024
da3faa5
style(pre-commit): autofix
pre-commit-ci[bot] Apr 11, 2024
4537ee0
Merge branch 'main' into feat/remake_ndt_scan_matcher_diag
YamatoAndo Apr 12, 2024
05c287d
[WIP] update
YamatoAndo Apr 12, 2024
350bb68
[WIP] update
YamatoAndo Apr 12, 2024
3603c73
style(pre-commit): autofix
pre-commit-ci[bot] Apr 15, 2024
d9a04d4
[WIP] update
YamatoAndo Apr 15, 2024
e4f98e6
fix typo
YamatoAndo Apr 15, 2024
366aa82
style(pre-commit): autofix
pre-commit-ci[bot] Apr 15, 2024
2b44237
[WIP] update
YamatoAndo Apr 15, 2024
9c8911e
style(pre-commit): autofix
pre-commit-ci[bot] Apr 15, 2024
1e2a693
update readme
YamatoAndo Apr 15, 2024
7cc312c
style(pre-commit): autofix
pre-commit-ci[bot] Apr 15, 2024
95791ba
[WIP] update
YamatoAndo Apr 15, 2024
847e633
style(pre-commit): autofix
pre-commit-ci[bot] Apr 15, 2024
e17e6ad
[WIP] update
YamatoAndo Apr 15, 2024
abfcfb8
fix typo
YamatoAndo Apr 15, 2024
6e354b1
style(pre-commit): autofix
pre-commit-ci[bot] Apr 15, 2024
ea7c941
[WIP] update
YamatoAndo Apr 15, 2024
20461f5
[WIP] udpate
YamatoAndo Apr 15, 2024
24c11d4
[WIP] udpate
YamatoAndo Apr 15, 2024
09e431e
style(pre-commit): autofix
pre-commit-ci[bot] Apr 15, 2024
d9d5acc
add is_need_rebuild
YamatoAndo Apr 16, 2024
a7617a9
fix typo
YamatoAndo Apr 16, 2024
7151e54
style(pre-commit): autofix
pre-commit-ci[bot] Apr 16, 2024
8808d72
add image
YamatoAndo Apr 16, 2024
98025b6
style(pre-commit): autofix
pre-commit-ci[bot] Apr 16, 2024
a4c68ff
remove unused func
YamatoAndo Apr 16, 2024
4399e46
fix
YamatoAndo Apr 16, 2024
4d2dbe8
fix typo
YamatoAndo Apr 16, 2024
bcf88e7
style(pre-commit): autofix
pre-commit-ci[bot] Apr 16, 2024
d7f2fec
update image
YamatoAndo Apr 16, 2024
fbcab26
fix typo
YamatoAndo Apr 16, 2024
57acd8a
fix
YamatoAndo Apr 16, 2024
9dddbb0
Merge branch 'main' into feat/remake_ndt_scan_matcher_diag
YamatoAndo Apr 16, 2024
51cdedc
remove unused include
YamatoAndo Apr 17, 2024
e410f63
move code
YamatoAndo Apr 17, 2024
cc16d4e
move code
YamatoAndo Apr 18, 2024
5f83918
style(pre-commit): autofix
pre-commit-ci[bot] Apr 18, 2024
f975f1b
fix FIX ME
YamatoAndo Apr 18, 2024
b196796
update
YamatoAndo Apr 19, 2024
3cd9471
remove unused func
YamatoAndo Apr 22, 2024
2008b63
[WIP] update
YamatoAndo Apr 22, 2024
7bfad46
add diag for trigger node service
YamatoAndo Apr 22, 2024
e0c1ddb
style(pre-commit): autofix
pre-commit-ci[bot] Apr 22, 2024
b9acf56
move code
YamatoAndo Apr 22, 2024
02e0434
update
YamatoAndo Apr 22, 2024
1e384e0
delete unused code
YamatoAndo Apr 22, 2024
bbd4017
style(pre-commit): autofix
pre-commit-ci[bot] Apr 22, 2024
7532630
udpate
YamatoAndo Apr 22, 2024
9fca75c
delete RCLCPP message
YamatoAndo Apr 22, 2024
b4e8c4b
style(pre-commit): autofix
pre-commit-ci[bot] Apr 22, 2024
2a1d3cb
update
YamatoAndo Apr 23, 2024
7448ceb
fix typo
YamatoAndo Apr 23, 2024
00ffef7
style(pre-commit): autofix
pre-commit-ci[bot] Apr 23, 2024
17d0f8d
Merge branch 'main' into feat/remake_ndt_scan_matcher_diag
YamatoAndo Apr 23, 2024
ed77431
rename diag
YamatoAndo Apr 23, 2024
fb55069
rename func
YamatoAndo Apr 23, 2024
e583fa8
style(pre-commit): autofix
pre-commit-ci[bot] Apr 23, 2024
e6eb57e
remove Transition condition to OK
YamatoAndo Apr 23, 2024
ce8adef
style(pre-commit): autofix
pre-commit-ci[bot] Apr 23, 2024
3ccfa24
fix table
YamatoAndo Apr 23, 2024
78b562c
update readme
YamatoAndo Apr 23, 2024
55cb19b
style(pre-commit): autofix
pre-commit-ci[bot] Apr 23, 2024
1a43e40
fix order
YamatoAndo Apr 23, 2024
5cc34d9
fix typo
YamatoAndo Apr 23, 2024
06aa1e5
remove diag prefix
YamatoAndo Apr 23, 2024
e741fc6
style(pre-commit): autofix
pre-commit-ci[bot] Apr 23, 2024
856cb9f
fix readme
YamatoAndo Apr 24, 2024
28bd5c6
style(pre-commit): autofix
pre-commit-ci[bot] Apr 24, 2024
d7b1947
rename diag
YamatoAndo Apr 24, 2024
fb79fc2
style(pre-commit): autofix
pre-commit-ci[bot] Apr 24, 2024
6769c79
remove unused code
YamatoAndo Apr 25, 2024
583ad25
fix double free
YamatoAndo Apr 25, 2024
fd147ff
style(pre-commit): autofix
pre-commit-ci[bot] Apr 25, 2024
c33adc9
fix typo
YamatoAndo Apr 26, 2024
2e60bef
output to terminal
YamatoAndo Apr 26, 2024
0012714
style(pre-commit): autofix
pre-commit-ci[bot] Apr 26, 2024
2a911c1
fix typo
YamatoAndo Apr 26, 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
Prev Previous commit
Next Next commit
move code
Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>
  • Loading branch information
YamatoAndo committed Apr 18, 2024
commit cc16d4eec46ab5bebf0a3bcfd741f5002d1a8451
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,9 @@ class MapUpdateModule
void callback_timer(
const bool is_activated, const std::optional<geometry_msgs::msg::Point> & position);

void process_callback_timer(
const bool is_activated, const std::optional<geometry_msgs::msg::Point> & position);

// Update the specified NDT
bool update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt);
void update_map(const geometry_msgs::msg::Point & position);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,8 @@ class NDTScanMatcher : public rclcpp::Node
void callback_regularization_pose(
geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);

void set_initial_pose(
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr);
bool set_input_source(
sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame);
bool process_scan_matching(
SakodaShintaro marked this conversation as resolved.
Show resolved Hide resolved
Expand Down
51 changes: 29 additions & 22 deletions localization/ndt_scan_matcher/src/map_update_module.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -66,38 +66,45 @@ void MapUpdateModule::initialize_diagnostics_key_value()

void MapUpdateModule::callback_timer(
const bool is_activated, const std::optional<geometry_msgs::msg::Point> & position)
{
process_callback_timer(is_activated, position);

diagnostics_map_update_->publish();
diagnostics_map_update_->clearLevelAndMessage();
}

void MapUpdateModule::process_callback_timer(
const bool is_activated, const std::optional<geometry_msgs::msg::Point> & position)
{
diagnostics_map_update_->addKeyValue("timer_callback_time_stamp", clock_->now().seconds());
diagnostics_map_update_->addKeyValue("is_activated", is_activated);

if (is_activated) {
const bool is_set_last_update_position = (position != std::nullopt);
diagnostics_map_update_->addKeyValue(
"is_set_last_update_position", is_set_last_update_position);

if (is_set_last_update_position) {
if (should_update_map(position.value())) {
RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)");
update_map(position.value());
}
} else {
std::stringstream message;
message << "Cannot find the reference position for map update."
<< "Please check if the EKF odometry is provided to NDT.";
diagnostics_map_update_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 1000, message.str());
}
} else {
diagnostics_map_update_->addKeyValue("is_activated", is_activated);
if (!is_activated) {
std::stringstream message;
message << "Node is not activated.";
RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 1000, message.str());
diagnostics_map_update_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return;
}

diagnostics_map_update_->publish();
diagnostics_map_update_->clearLevelAndMessage();
const bool is_set_last_update_position = (position != std::nullopt);
diagnostics_map_update_->addKeyValue(
"is_set_last_update_position", is_set_last_update_position);
if (!is_set_last_update_position) {
std::stringstream message;
message << "Cannot find the reference position for map update."
<< "Please check if the EKF odometry is provided to NDT.";
diagnostics_map_update_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 1000, message.str());
return;
}

if (should_update_map(position.value())) {
RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)");
update_map(position.value());
}
}

bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position)
Expand Down
153 changes: 81 additions & 72 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,134 +69,134 @@
tf2_broadcaster_(*this),
tf2_buffer_(this->get_clock()),
tf2_listener_(tf2_buffer_),
ndt_ptr_(new NormalDistributionsTransform),
is_activated_(false),
param_(this)
{
timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group =
this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::CallbackGroup::SharedPtr sensor_callback_group =
this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

auto initial_pose_sub_opt = rclcpp::SubscriptionOptions();
initial_pose_sub_opt.callback_group = initial_pose_callback_group;
auto sensor_sub_opt = rclcpp::SubscriptionOptions();
sensor_sub_opt.callback_group = sensor_callback_group;

constexpr double map_update_dt = 1.0;
constexpr auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(map_update_dt));
map_update_timer_ = rclcpp::create_timer(
this, this->get_clock(), period_ns, std::bind(&NDTScanMatcher::callback_timer, this),
timer_callback_group_);
initial_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"ekf_pose_with_covariance", 100,
std::bind(&NDTScanMatcher::callback_initial_pose, this, std::placeholders::_1),
initial_pose_sub_opt);
sensor_points_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"points_raw", rclcpp::SensorDataQoS().keep_last(1),
std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1),
sensor_sub_opt);

// Only if regularization is enabled, subscribe to the regularization base pose
if (param_.ndt_regularization_enable) {
// NOTE: The reason that the regularization subscriber does not belong to the
// sensor_callback_group is to ensure that the regularization callback is called even if
// sensor_callback takes long time to process.
// Both callback_initial_pose and callback_regularization_pose must not miss receiving data for
// proper interpolation.
regularization_pose_sub_ =
this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"regularization_pose_with_covariance", 10,
std::bind(&NDTScanMatcher::callback_regularization_pose, this, std::placeholders::_1),
initial_pose_sub_opt);
const double value_as_unlimited = 1000.0;
regularization_pose_buffer_ =
std::make_unique<SmartPoseBuffer>(this->get_logger(), value_as_unlimited, value_as_unlimited);

diagnostics_regularization_pose_ =
std::make_unique<DiagnosticsModule>(this, "localization", "regularization_pose_callback");
}

sensor_aligned_pose_pub_ =
this->create_publisher<sensor_msgs::msg::PointCloud2>("points_aligned", 10);
no_ground_points_aligned_pose_pub_ =
this->create_publisher<sensor_msgs::msg::PointCloud2>("points_aligned_no_ground", 10);
ndt_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("ndt_pose", 10);
ndt_pose_with_covariance_pub_ =
this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"ndt_pose_with_covariance", 10);
initial_pose_with_covariance_pub_ =
this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"initial_pose_with_covariance", 10);
multi_ndt_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseArray>("multi_ndt_pose", 10);
multi_initial_pose_pub_ =
this->create_publisher<geometry_msgs::msg::PoseArray>("multi_initial_pose", 10);
exe_time_pub_ = this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>("exe_time_ms", 10);
transform_probability_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>("transform_probability", 10);
nearest_voxel_transformation_likelihood_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
"nearest_voxel_transformation_likelihood", 10);
no_ground_transform_probability_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
"no_ground_transform_probability", 10);
no_ground_nearest_voxel_transformation_likelihood_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
"no_ground_nearest_voxel_transformation_likelihood", 10);
iteration_num_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Int32Stamped>("iteration_num", 10);
initial_to_result_relative_pose_pub_ =
this->create_publisher<geometry_msgs::msg::PoseStamped>("initial_to_result_relative_pose", 10);
initial_to_result_distance_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>("initial_to_result_distance", 10);
initial_to_result_distance_old_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
"initial_to_result_distance_old", 10);
initial_to_result_distance_new_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
"initial_to_result_distance_new", 10);
ndt_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("ndt_marker", 10);
ndt_monte_carlo_initial_pose_marker_pub_ =
this->create_publisher<visualization_msgs::msg::MarkerArray>(
"monte_carlo_initial_pose_marker", 10);

service_ = this->create_service<tier4_localization_msgs::srv::PoseWithCovarianceStamped>(
"ndt_align_srv",
std::bind(
&NDTScanMatcher::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2),
rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group);
service_trigger_node_ = this->create_service<std_srvs::srv::SetBool>(
"trigger_node_srv",
std::bind(
&NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2),
rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group);

ndt_ptr_->setParams(param_.ndt);

initial_pose_buffer_ = std::make_unique<SmartPoseBuffer>(
this->get_logger(), param_.validation.initial_pose_timeout_sec,
param_.validation.initial_pose_distance_tolerance_m);

map_update_module_ =
std::make_unique<MapUpdateModule>(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading);

diagnostics_scan_points_ =
std::make_unique<DiagnosticsModule>(this, "localization", "sensor_points_callback");

diagnostics_initial_pose_ =
std::make_unique<DiagnosticsModule>(this, "localization", "initial_pose_callback");

diagnostics_ndt_align_ =
std::make_unique<DiagnosticsModule>(this, "localization", "ndt_align_service");

Check warning on line 193 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: Large Method

NDTScanMatcher::NDTScanMatcher increases from 106 to 110 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
}

void NDTScanMatcher::callback_timer()
{
map_update_module_->callback_timer(is_activated_, latest_ekf_position_);

Check notice on line 199 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)

✅ No longer an issue: Complex Method

NDTScanMatcher::publish_diagnostic is no longer above the threshold for cyclomatic complexity. 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.

Check notice on line 199 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)

✅ No longer an issue: Complex Conditional

NDTScanMatcher::publish_diagnostic no longer has a complex conditional
}

void NDTScanMatcher::callback_initial_pose(
Expand All @@ -204,38 +204,52 @@
{
diagnostics_initial_pose_->clear();

set_initial_pose(initial_pose_msg_ptr);

diagnostics_initial_pose_->publish();
}


void NDTScanMatcher::set_initial_pose(
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr)
{
diagnostics_initial_pose_->addKeyValue(
"topic_time_stamp", static_cast<rclcpp::Time>(initial_pose_msg_ptr->header.stamp).seconds());
diagnostics_initial_pose_->addKeyValue("is_activated", static_cast<bool>(is_activated_));

if (is_activated_) {
if (initial_pose_msg_ptr->header.frame_id == param_.frame.map_frame) {
initial_pose_buffer_->push_back(initial_pose_msg_ptr);
} else {
std::stringstream message;
message << "Received initial pose message with frame_id "
<< initial_pose_msg_ptr->header.frame_id << ", but expected "
<< param_.frame.map_frame
<< ". Please check the frame_id in the input topic and ensure it is correct.";
diagnostics_initial_pose_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
RCLCPP_ERROR_STREAM_THROTTLE(get_logger(), *this->get_clock(), 1000, message.str());
}

{
// latest_ekf_position_ is also used by callback_timer, so it is necessary to acquire the lock
std::lock_guard<std::mutex> lock(latest_ekf_position_mtx_);
latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position;
}
} else {
// check is_activated
diagnostics_initial_pose_->addKeyValue("is_activated", static_cast<bool>(is_activated_));
if (!is_activated_) {
std::stringstream message;
message << "Node is not activated.";
RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *this->get_clock(), 1000, message.str());
diagnostics_initial_pose_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return;
}

// check is_expected_frame_id
const bool is_expected_frame_id = (initial_pose_msg_ptr->header.frame_id == param_.frame.map_frame);
diagnostics_initial_pose_->addKeyValue("is_expected_frame_id", is_expected_frame_id);
if (!is_expected_frame_id) {
std::stringstream message;
message << "Received initial pose message with frame_id "
<< initial_pose_msg_ptr->header.frame_id << ", but expected "
<< param_.frame.map_frame
<< ". Please check the frame_id in the input topic and ensure it is correct.";
diagnostics_initial_pose_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
RCLCPP_ERROR_STREAM_THROTTLE(get_logger(), *this->get_clock(), 1000, message.str());
return;
}

initial_pose_buffer_->push_back(initial_pose_msg_ptr);

{
// latest_ekf_position_ is also used by callback_timer, so it is necessary to acquire the lock
std::lock_guard<std::mutex> lock(latest_ekf_position_mtx_);
latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position;
}

diagnostics_initial_pose_->publish();
}

void NDTScanMatcher::callback_regularization_pose(
Expand All @@ -253,383 +267,378 @@
void NDTScanMatcher::callback_sensor_points(
sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame)
{
// clear diagnostics
diagnostics_scan_points_->clear();
diagnostics_scan_points_->addKeyValue("topic_time_stamp", 0.0);
diagnostics_scan_points_->addKeyValue("is_activated", false);
diagnostics_scan_points_->addKeyValue("is_set_map_points", false);
diagnostics_scan_points_->addKeyValue("is_set_sensor_points", false);
diagnostics_scan_points_->addKeyValue("sensor_points_size", 0);
diagnostics_scan_points_->addKeyValue("sensor_points_delay_time_sec", 0.0);
diagnostics_scan_points_->addKeyValue("sensor_points_max_distance", 0.0);
diagnostics_scan_points_->addKeyValue("is_succeed_interpolate_initial_pose", false);
diagnostics_scan_points_->addKeyValue("iteration_num", 0);
diagnostics_scan_points_->addKeyValue("local_optimal_solution_oscillation_num", 0);
diagnostics_scan_points_->addKeyValue("transform_probability", 0.0);
diagnostics_scan_points_->addKeyValue("nearest_voxel_transformation_likelihood", 0.0);
diagnostics_scan_points_->addKeyValue("distance_initial_to_result", 0.0);
diagnostics_scan_points_->addKeyValue("execution_time", 0.0);
diagnostics_scan_points_->addKeyValue("skipping_publish_num", 0);

// check topic_time_stamp
diagnostics_scan_points_->addKeyValue(
"topic_time_stamp",
static_cast<rclcpp::Time>(sensor_points_msg_in_sensor_frame->header.stamp).seconds());
// scan matching
const bool is_succeed_scan_matching = process_scan_matching(sensor_points_msg_in_sensor_frame);

// check is_activated
diagnostics_scan_points_->addKeyValue("is_activated", is_activated_);
if (!is_activated_) {
// check skipping_publish_num
static size_t skipping_publish_num = 0;
const size_t error_skipping_publish_num = 5;
skipping_publish_num = is_succeed_scan_matching ? 0 : (skipping_publish_num + 1);
diagnostics_scan_points_->addKeyValue("skipping_publish_num", skipping_publish_num);
if (skipping_publish_num > 0 && skipping_publish_num < error_skipping_publish_num) {
std::stringstream message;
message << "Node is not activated.";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
message << "skipping_publish_num > 0 (" << skipping_publish_num << " times).";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
}

const bool is_set_sensor_points = set_input_source(sensor_points_msg_in_sensor_frame);

// check is_set_sensor_points
diagnostics_scan_points_->addKeyValue("is_set_sensor_points", is_set_sensor_points);
if (!is_set_sensor_points) {
else if (skipping_publish_num >= error_skipping_publish_num) {
std::stringstream message;
message << "Sensor points is not set.";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times).";
RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
}

if (is_activated_) {
bool is_published_topic = false;

if (is_set_sensor_points) {
is_published_topic = process_scan_matching(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;
skipping_publish_num = is_published_topic ? 0 : (skipping_publish_num + 1);
diagnostics_scan_points_->addKeyValue("skipping_publish_num", skipping_publish_num);
if (skipping_publish_num > 0 && skipping_publish_num < error_skipping_publish_num) {
std::stringstream message;
message << "skipping_publish_num > 0 (" << skipping_publish_num << " times).";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
}
else if (skipping_publish_num >= error_skipping_publish_num) {
std::stringstream message;
message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times).";
RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
}
diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
}

diagnostics_scan_points_->publish();
}

bool NDTScanMatcher::set_input_source(
sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame)
{
std::lock_guard<std::mutex> lock(ndt_ptr_mtx_);

// check sensor_points_size
const size_t sensor_points_size = sensor_points_msg_in_sensor_frame->width;
diagnostics_scan_points_->addKeyValue("sensor_points_size", sensor_points_size);
if (sensor_points_size == 0) {
std::stringstream message;
message << "Sensor points is empty.";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return false;
}

// check sensor_points_delay_time_sec
const double sensor_points_delay_time_sec = (this->now() - sensor_points_msg_in_sensor_frame->header.stamp).seconds();
diagnostics_scan_points_->addKeyValue(
"sensor_points_delay_time_sec", sensor_points_delay_time_sec);
if (sensor_points_delay_time_sec > param_.validation.lidar_topic_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]).";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());

// If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer,
// even if further processing continues, the estimated result will be rejected by ekf_localizer.
// Therefore, it would be acceptable to exit the function here.
// However, for now, we will continue the processing as it is.

// return false;
}

// preprocess input pointcloud
pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_in_sensor_frame(
new pcl::PointCloud<PointSource>);
pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_in_baselink_frame(
new pcl::PointCloud<PointSource>);
const std::string & sensor_frame = sensor_points_msg_in_sensor_frame->header.frame_id;

pcl::fromROSMsg(*sensor_points_msg_in_sensor_frame, *sensor_points_in_sensor_frame);
transform_sensor_measurement(
sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame,
sensor_points_in_baselink_frame);


// check sensor_points_max_distance
double max_distance = 0.0;
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);
}

diagnostics_scan_points_->addKeyValue("sensor_points_max_distance", max_distance);
if (max_distance < param_.sensor_points.required_distance) {
std::stringstream message;
message << "Max distance of sensor points = " << std::fixed << std::setprecision(3)
<< max_distance << " [m] < " << param_.sensor_points.required_distance << " [m]";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return false;
}

// set sensor points to ndt class
std::lock_guard<std::mutex> lock(ndt_ptr_mtx_);
ndt_ptr_->setInputSource(sensor_points_in_baselink_frame);

return true;
}

bool NDTScanMatcher::process_scan_matching(
sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame)
{
std::lock_guard<std::mutex> lock(ndt_ptr_mtx_);

const auto exe_start_time = std::chrono::system_clock::now();

// check topic_time_stamp
const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp;
diagnostics_scan_points_->addKeyValue("topic_time_stamp",sensor_ros_time.seconds());

// set sensor_points to ndt
set_input_source(sensor_points_msg_in_sensor_frame);

std::lock_guard<std::mutex> lock(ndt_ptr_mtx_);

// check is_set_sensor_points
const auto sensor_points_in_baselink_frame = ndt_ptr_->getInputSource();
const bool is_set_sensor_points = (sensor_points_in_baselink_frame != nullptr);
diagnostics_scan_points_->addKeyValue("is_set_sensor_points", is_set_sensor_points);
if (!is_set_sensor_points) {
std::stringstream message;
message << "Sensor points is not set.";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return false;
}

// check is_activated
diagnostics_scan_points_->addKeyValue("is_activated", static_cast<bool>(is_activated_));
if (!is_activated_) {
std::stringstream message;
message << "Node is not activated.";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return false;
}

// calculate initial pose
std::optional<SmartPoseBuffer::InterpolateResult> interpolation_result_opt =
initial_pose_buffer_->interpolate(sensor_ros_time);

// check is_succeed_interpolate_initial_pose
const bool is_succeed_interpolate_initial_pose = (interpolation_result_opt != std::nullopt);
diagnostics_scan_points_->addKeyValue("is_succeed_interpolate_initial_pose", is_succeed_interpolate_initial_pose);
if (!is_succeed_interpolate_initial_pose) {
std::stringstream message;
message << "Couldn't interpolate pose. Please check the initial pose topic";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return false;
}

initial_pose_buffer_->pop_old(sensor_ros_time);
const SmartPoseBuffer::InterpolateResult & interpolation_result =
interpolation_result_opt.value();

// if regularization is enabled and available, set pose to NDT for regularization
if (param_.ndt_regularization_enable) {
add_regularization_pose(sensor_ros_time);
}

// check is_set_map_points
const bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr);
diagnostics_scan_points_->addKeyValue("is_set_map_points", is_set_map_points);
if (!is_set_map_points) {
std::stringstream message;
message << "Map points is not set.";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return false;
}


// perform ndt scan matching
const Eigen::Matrix4f initial_pose_matrix =
pose_to_matrix4f(interpolation_result.interpolated_pose.pose.pose);
auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
ndt_ptr_->align(*output_cloud, initial_pose_matrix);
const pclomp::NdtResult ndt_result = ndt_ptr_->getResult();

const geometry_msgs::msg::Pose result_pose_msg = matrix4f_to_pose(ndt_result.pose);
std::vector<geometry_msgs::msg::Pose> transformation_msg_array;
for (const auto & pose_matrix : ndt_result.transformation_array) {
geometry_msgs::msg::Pose pose_ros = matrix4f_to_pose(pose_matrix);
transformation_msg_array.push_back(pose_ros);
}

// check iteration_num
diagnostics_scan_points_->addKeyValue("iteration_num", ndt_result.iteration_num);
const bool is_ok_iteration_num = (ndt_result.iteration_num < ndt_ptr_->getMaximumIterations());
if (!is_ok_iteration_num) {
std::stringstream message;
message << "The number of iterations has reached its upper limit. The number of iterations: "
<< ndt_result.iteration_num << ", Limit: " << ndt_ptr_->getMaximumIterations() << ".";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
}

// check local_optimal_solution_oscillation_num
constexpr int oscillation_num_threshold = 10;
const int oscillation_num = count_oscillation(transformation_msg_array);
diagnostics_scan_points_->addKeyValue(
"local_optimal_solution_oscillation_num", oscillation_num);
const bool is_local_optimal_solution_oscillation = (oscillation_num > oscillation_num_threshold);
if (is_local_optimal_solution_oscillation) {
std::stringstream message;
message << "There is a possibility of oscillation in a local minimum";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
}

// check score
diagnostics_scan_points_->addKeyValue("transform_probability", ndt_result.transform_probability);
diagnostics_scan_points_->addKeyValue(
"nearest_voxel_transformation_likelihood", ndt_result.nearest_voxel_transformation_likelihood);
std::string score_name = "";
double score = 0.0;
double score_threshold = 0.0;
if (param_.score_estimation.converged_param_type == ConvergedParamType::TRANSFORM_PROBABILITY) {
score_name = "Transform Probability";
score = ndt_result.transform_probability;
score_threshold = param_.score_estimation.converged_param_transform_probability;
}
else if (param_.score_estimation.converged_param_type == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) {
score_name = "Nearest Voxel Transformation Likelihood";
score = ndt_result.nearest_voxel_transformation_likelihood;
score_threshold = param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood;
}
else {
std::stringstream message;
message << "Unknown converged param type. Please check `score_estimation.converged_param_type`";
RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
return false;
}

bool is_ok_score = (score > score_threshold);
if (!is_ok_score) {
std::stringstream message;
message << "Transform Probability" << " is below the threshold. Score: " << score
<< ", Threshold: " << score_threshold << ".";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
}

// check is_converged
bool is_converged =
(is_ok_iteration_num || is_local_optimal_solution_oscillation) && is_ok_score;

// covariance estimation
const Eigen::Quaterniond map_to_base_link_quat = Eigen::Quaterniond(
result_pose_msg.orientation.w, result_pose_msg.orientation.x, result_pose_msg.orientation.y,
result_pose_msg.orientation.z);
const Eigen::Matrix3d map_to_base_link_rotation =
map_to_base_link_quat.normalized().toRotationMatrix();

std::array<double, 36> ndt_covariance =
rotate_covariance(param_.covariance.output_pose_covariance, map_to_base_link_rotation);

if (is_converged && param_.covariance.covariance_estimation.enable) {
const auto estimated_covariance =
estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time);
ndt_covariance = estimated_covariance;
}

// check distance_initial_to_result
const auto distance_initial_to_result = static_cast<double>(
norm(interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position));
diagnostics_scan_points_->addKeyValue("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) {
std::stringstream message;
message << "distance_initial_to_result is too large (" << distance_initial_to_result
<< " [m]).";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
}

// check execution_time
const auto exe_end_time = std::chrono::system_clock::now();
const auto duration_micro_sec =
std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
const auto exe_time = static_cast<float>(duration_micro_sec) / 1000.0f;
diagnostics_scan_points_->addKeyValue("execution_time", exe_time);
if (exe_time > param_.validation.critical_upper_bound_exe_time_ms) {
std::stringstream message;
message << "NDT exe time is too long (took " << exe_time << " [ms]).";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10, message.str());
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
}


// publish
initial_pose_with_covariance_pub_->publish(interpolation_result.interpolated_pose);
exe_time_pub_->publish(make_float32_stamped(sensor_ros_time, exe_time));
transform_probability_pub_->publish(
make_float32_stamped(sensor_ros_time, ndt_result.transform_probability));
nearest_voxel_transformation_likelihood_pub_->publish(
make_float32_stamped(sensor_ros_time, ndt_result.nearest_voxel_transformation_likelihood));
iteration_num_pub_->publish(make_int32_stamped(sensor_ros_time, ndt_result.iteration_num));
publish_tf(sensor_ros_time, result_pose_msg);
publish_pose(sensor_ros_time, result_pose_msg, ndt_covariance, is_converged);
publish_marker(sensor_ros_time, transformation_msg_array);
publish_initial_to_result(
sensor_ros_time, result_pose_msg, interpolation_result.interpolated_pose,
interpolation_result.old_pose, interpolation_result.new_pose);

pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_in_map_ptr(
new pcl::PointCloud<PointSource>);
tier4_autoware_utils::transformPointCloud(
*sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose);
publish_point_cloud(sensor_ros_time, param_.frame.map_frame, sensor_points_in_map_ptr);

// whether use no ground points to calculate score
if (param_.score_estimation.no_ground_points.enable) {
// remove ground
pcl::shared_ptr<pcl::PointCloud<PointSource>> no_ground_points_in_map_ptr(
new pcl::PointCloud<PointSource>);
for (std::size_t i = 0; i < sensor_points_in_map_ptr->size(); i++) {
const float point_z = sensor_points_in_map_ptr->points[i].z; // NOLINT
if (
point_z - matrix4f_to_pose(ndt_result.pose).position.z >
param_.score_estimation.no_ground_points.z_margin_for_ground_removal) {
no_ground_points_in_map_ptr->points.push_back(sensor_points_in_map_ptr->points[i]);
}
}
// pub remove-ground points
sensor_msgs::msg::PointCloud2 no_ground_points_msg_in_map;
pcl::toROSMsg(*no_ground_points_in_map_ptr, no_ground_points_msg_in_map);
no_ground_points_msg_in_map.header.stamp = sensor_ros_time;
no_ground_points_msg_in_map.header.frame_id = param_.frame.map_frame;
no_ground_points_aligned_pose_pub_->publish(no_ground_points_msg_in_map);
// calculate score
const auto no_ground_transform_probability = static_cast<float>(
ndt_ptr_->calculateTransformationProbability(*no_ground_points_in_map_ptr));
const auto no_ground_nearest_voxel_transformation_likelihood = static_cast<float>(
ndt_ptr_->calculateNearestVoxelTransformationLikelihood(*no_ground_points_in_map_ptr));
// pub score
no_ground_transform_probability_pub_->publish(
make_float32_stamped(sensor_ros_time, no_ground_transform_probability));
no_ground_nearest_voxel_transformation_likelihood_pub_->publish(
make_float32_stamped(sensor_ros_time, no_ground_nearest_voxel_transformation_likelihood));
}

return is_converged;

Check notice on line 641 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)

✅ No longer an issue: Complex Method

NDTScanMatcher::callback_sensor_points is no longer above the threshold for cyclomatic complexity. 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.

Check warning on line 641 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)

❌ New issue: Complex Method

NDTScanMatcher::process_scan_matching has a cyclomatic complexity of 21, 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.
}

void NDTScanMatcher::transform_sensor_measurement(
Expand Down Expand Up @@ -1127,8 +1136,8 @@

output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg);
RCLCPP_DEBUG_STREAM(get_logger(), "best_score," << best_particle_ptr->score);
diagnostics_ndt_align_->addKeyValue(
"latest_ndt_align_service_best_score", best_particle_ptr->score);

Check warning on line 1140 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: Large Method

NDTScanMatcher::align_pose increases from 93 to 95 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

return result_pose_with_cov_msg;
}
Loading