Skip to content

Commit

Permalink
fix(ndt_scan_matcher): delete diagnostics thread
Browse files Browse the repository at this point in the history 
Signed-off-by: yamato-ando <Yamato ANDO>
  • Loading branch information
yamato-ando authored and yamato-ando committed Nov 9, 2023
1 parent 26fdcf5 commit c6f2e80
Show file tree
Hide file tree
Showing 4 changed files with 68 additions and 86 deletions.
4 changes: 1 addition & 3 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
View file
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
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
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
Original file line number Diff line number Diff line change
Expand Up @@ -280,93 +280,84 @@ NDTScanMatcher::NDTScanMatcher()
&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 @@ void NDTScanMatcher::callback_sensor_points(
}

// 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 @@ void NDTScanMatcher::callback_sensor_points(
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 @@ void NDTScanMatcher::callback_sensor_points(
(*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 @@ void NDTScanMatcher::service_trigger_node(
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 @@ void NDTScanMatcher::service_ndt_align(
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

0 comments on commit c6f2e80

Please sign in to comment.