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

fix(ndt_scan_matcher): delete diagnostics thread #5532

Merged
merged 1 commit into from
Nov 9, 2023
Merged

fix(ndt_scan_matcher): delete diagnostics thread #5532

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
4 changes: 1 addition & 3 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,7 @@ class MapUpdateModule
rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
rclcpp::CallbackGroup::SharedPtr main_callback_group,
std::shared_ptr<std::map<std::string, std::string>> state_ptr);
rclcpp::CallbackGroup::SharedPtr main_callback_group);

private:
friend class NDTScanMatcher;
Expand Down Expand Up @@ -82,7 +81,6 @@ class MapUpdateModule
rclcpp::Logger logger_;
rclcpp::Clock::SharedPtr clock_;
std::shared_ptr<Tf2ListenerModule> tf2_listener_module_;
std::shared_ptr<std::map<std::string, std::string>> state_ptr_;

std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;
Expand Down
4 changes: 1 addition & 3 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,7 +132,7 @@ class NDTScanMatcher : public rclcpp::Node
const rclcpp::Time & sensor_ros_time);
void add_regularization_pose(const rclcpp::Time & sensor_ros_time);

void timer_diagnostic();
void publish_diagnostic();

rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_sub_;
Expand Down Expand Up @@ -203,8 +203,6 @@ class NDTScanMatcher : public rclcpp::Node
std::mutex ndt_ptr_mtx_;
std::mutex initial_pose_array_mtx_;

std::thread diagnostic_thread_;

// variables for regularization
const bool regularization_enabled_;
std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr>
Expand Down
4 changes: 1 addition & 3 deletions localization/ndt_scan_matcher/src/map_update_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,15 +26,13 @@ MapUpdateModule::MapUpdateModule(
rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
rclcpp::CallbackGroup::SharedPtr main_callback_group,
std::shared_ptr<std::map<std::string, std::string>> state_ptr)
rclcpp::CallbackGroup::SharedPtr main_callback_group)
: ndt_ptr_(std::move(ndt_ptr)),
ndt_ptr_mutex_(ndt_ptr_mutex),
map_frame_(std::move(map_frame)),
logger_(node->get_logger()),
clock_(node->get_clock()),
tf2_listener_module_(std::move(tf2_listener_module)),
state_ptr_(std::move(state_ptr)),
dynamic_map_loading_update_distance_(
node->declare_parameter<double>("dynamic_map_loading_update_distance")),
dynamic_map_loading_map_radius_(
Expand Down
142 changes: 65 additions & 77 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 @@ -280,93 +280,84 @@
&NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2),
rclcpp::ServicesQoS().get_rmw_qos_profile(), main_callback_group);

diagnostic_thread_ = std::thread(&NDTScanMatcher::timer_diagnostic, this);
diagnostic_thread_.detach();

tf2_listener_module_ = std::make_shared<Tf2ListenerModule>(this);

use_dynamic_map_loading_ = this->declare_parameter<bool>("use_dynamic_map_loading");
if (use_dynamic_map_loading_) {
map_update_module_ = std::make_unique<MapUpdateModule>(
this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group,
state_ptr_);
this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group);
} else {
map_module_ = std::make_unique<MapModule>(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group);
}

logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
}

void NDTScanMatcher::timer_diagnostic()
void NDTScanMatcher::publish_diagnostic()

Check warning on line 296 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L296 

Added line #L296 was not covered by tests
{
rclcpp::Rate rate(100);
while (rclcpp::ok()) {
diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;
diag_status_msg.name = "ndt_scan_matcher";
diag_status_msg.hardware_id = "";

for (const auto & key_value : (*state_ptr_)) {
diagnostic_msgs::msg::KeyValue key_value_msg;
key_value_msg.key = key_value.first;
key_value_msg.value = key_value.second;
diag_status_msg.values.push_back(key_value_msg);
}

diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;

Check warning on line 298 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L298 

Added line #L298 was not covered by tests
diag_status_msg.name = "ndt_scan_matcher";
diag_status_msg.hardware_id = "";

for (const auto & key_value : (*state_ptr_)) {
diagnostic_msgs::msg::KeyValue key_value_msg;
key_value_msg.key = key_value.first;
key_value_msg.value = key_value.second;
diag_status_msg.values.push_back(key_value_msg);
}

Check warning on line 307 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L307 

Added line #L307 was not covered by tests

diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
diag_status_msg.message = "";
if (state_ptr_->count("state") && (*state_ptr_)["state"] == "Initializing") {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message += "Initializing State. ";
}
if (

Check warning on line 315 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L315 

Added line #L315 was not covered by tests
state_ptr_->count("lidar_topic_delay_time_sec") &&
std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. ";
}
if (

Check warning on line 321 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L321 

Added line #L321 was not covered by tests
state_ptr_->count("skipping_publish_num") &&
std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 &&

Check notice on line 323 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 Conditional 

NDTScanMatcher::publish_diagnostic has 1 complex conditionals with 2 branches, threshold = 2. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.
std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message += "skipping_publish_num > 1. ";
}
if (

Check warning on line 328 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L328 

Added line #L328 was not covered by tests
state_ptr_->count("skipping_publish_num") &&
std::stoi((*state_ptr_)["skipping_publish_num"]) >= 5) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
diag_status_msg.message += "skipping_publish_num exceed limit. ";
}
if (

Check warning on line 334 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L334 

Added line #L334 was not covered by tests
state_ptr_->count("nearest_voxel_transformation_likelihood") &&
std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) <
converged_param_nearest_voxel_transformation_likelihood_) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message += "NDT score is unreliably low. ";
}
if (

Check warning on line 341 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L341 

Added line #L341 was not covered by tests
state_ptr_->count("execution_time") &&
std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message +=
"NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])";
}
// Ignore local optimal solution
if (

Check warning on line 349 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L349 

Added line #L349 was not covered by tests
state_ptr_->count("is_local_optimal_solution_oscillation") &&
std::stoi((*state_ptr_)["is_local_optimal_solution_oscillation"])) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
diag_status_msg.message = "";
if (state_ptr_->count("state") && (*state_ptr_)["state"] == "Initializing") {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message += "Initializing State. ";
}
if (
state_ptr_->count("lidar_topic_delay_time_sec") &&
std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. ";
}
if (
state_ptr_->count("skipping_publish_num") &&
std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 &&
std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message += "skipping_publish_num > 1. ";
}
if (
state_ptr_->count("skipping_publish_num") &&
std::stoi((*state_ptr_)["skipping_publish_num"]) >= 5) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
diag_status_msg.message += "skipping_publish_num exceed limit. ";
}
if (
state_ptr_->count("nearest_voxel_transformation_likelihood") &&
std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) <
converged_param_nearest_voxel_transformation_likelihood_) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message += "NDT score is unreliably low. ";
}
if (
state_ptr_->count("execution_time") &&
std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message +=
"NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])";
}
// Ignore local optimal solution
if (
state_ptr_->count("is_local_optimal_solution_oscillation") &&
std::stoi((*state_ptr_)["is_local_optimal_solution_oscillation"])) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
diag_status_msg.message = "local optimal solution oscillation occurred";
}

diagnostic_msgs::msg::DiagnosticArray diag_msg;
diag_msg.header.stamp = this->now();
diag_msg.status.push_back(diag_status_msg);
diag_status_msg.message = "local optimal solution oscillation occurred";

Check notice on line 353 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::timer_diagnostic no longer has a complex conditional. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.
}

diagnostics_pub_->publish(diag_msg);
diagnostic_msgs::msg::DiagnosticArray diag_msg;
diag_msg.header.stamp = this->now();
diag_msg.status.push_back(diag_status_msg);

rate.sleep();
}
diagnostics_pub_->publish(diag_msg);

Check notice on line 360 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::timer_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 360 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::publish_diagnostic has a cyclomatic complexity of 17, 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::callback_initial_pose(
Expand Down Expand Up @@ -476,13 +467,11 @@
}

// perform ndt scan matching
(*state_ptr_)["state"] = "Aligning";
const Eigen::Matrix4f initial_pose_matrix =
pose_to_matrix4f(interpolator.get_current_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();
(*state_ptr_)["state"] = "Sleeping";

const geometry_msgs::msg::Pose result_pose_msg = matrix4f_to_pose(ndt_result.pose);
std::vector<geometry_msgs::msg::Pose> transformation_msg_array;
Expand Down Expand Up @@ -573,6 +562,7 @@
make_float32_stamped(sensor_ros_time, no_ground_nearest_voxel_transformation_likelihood));
}

(*state_ptr_)["state"] = "Aligned";
(*state_ptr_)["transform_probability"] = std::to_string(ndt_result.transform_probability);
(*state_ptr_)["nearest_voxel_transformation_likelihood"] =
std::to_string(ndt_result.nearest_voxel_transformation_likelihood);
Expand All @@ -584,6 +574,8 @@
(*state_ptr_)["is_local_optimal_solution_oscillation"] = "0";
}
(*state_ptr_)["execution_time"] = std::to_string(exe_time);

publish_diagnostic();
}

void NDTScanMatcher::transform_sensor_measurement(
Expand Down Expand Up @@ -859,8 +851,6 @@
if (is_activated_) {
std::lock_guard<std::mutex> initial_pose_array_lock(initial_pose_array_mtx_);
initial_pose_msg_ptr_array_.clear();
} else {
(*state_ptr_)["state"] = "Initializing";
}
res->success = true;
}
Expand Down Expand Up @@ -897,9 +887,7 @@
return;
}

(*state_ptr_)["state"] = "Aligning";
res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame);
(*state_ptr_)["state"] = "Sleeping";
res->success = true;
res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance;
}
Expand Down
Loading