Skip to content

Commit

Permalink
fix(ndt_scan_matcher): delete diagnostics thread (autowarefoundation#…
Browse files Browse the repository at this point in the history 
…5532)

Signed-off-by: yamato-ando <Yamato ANDO>
Co-authored-by: yamato-ando <Yamato ANDO>
  • Loading branch information
@YamatoAndo @shmpwk
YamatoAndo authored and shmpwk committed Nov 16, 2023
1 parent 59951ad commit 847a661
Show file tree
Hide file tree
Showing 4 changed files with 67 additions and 85 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 @@ -126,7 +126,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 @@ -193,8 +193,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
140 changes: 64 additions & 76 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 @@ -236,91 +236,82 @@ 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);
}
}

void NDTScanMatcher::timer_diagnostic()
void NDTScanMatcher::publish_diagnostic()
{
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;
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);
}

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 = "";
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";
}

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);
}

void NDTScanMatcher::callback_initial_pose(
Expand Down Expand Up @@ -430,13 +421,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 auto exe_end_time = std::chrono::system_clock::now();
const auto duration_micro_sec =
Expand Down Expand Up @@ -519,6 +508,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 @@ -530,6 +520,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 @@ -733,8 +725,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 @@ -771,9 +761,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 847a661

Please sign in to comment.