diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index 619a0b8520b97..6030ea2fe2a9c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -65,9 +65,9 @@ #include #include -#include #include +#include #include #include #include @@ -138,7 +138,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::TimerBase::SharedPtr timer_; - diagnostic_updater::Updater updater_{this}; + rclcpp::Publisher::SharedPtr diagnostics_pub_; const std::string input_twist_topic_type_; @@ -180,7 +180,9 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); void timer_callback(); - void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + void checkConcatStatus(); + int consecutive_concatenate_failures{0}; + std::string replaceSyncTopicNamePostfix( const std::string & original_topic_name, const std::string & postfix); diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 8adea1ea1467b..56b9070f318a8 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -218,9 +218,8 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro // Diagnostic Updater { - updater_.setHardwareID("concatenate_data_checker"); - updater_.add( - "concat_status", this, &PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus); + diagnostics_pub_ = + this->create_publisher("/diagnostics", 10); } } @@ -462,7 +461,7 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() } } - updater_.force_update(); + checkConcatStatus(); cloud_stdmap_ = cloud_stdmap_tmp_; std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { @@ -712,21 +711,45 @@ void PointCloudConcatenateDataSynchronizerComponent::odom_callback( twist_ptr_queue_.push_back(twist_ptr); } -void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat) +void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus() { + diagnostic_msgs::msg::DiagnosticStatus diag_status_msg; + diag_status_msg.name = std::string(this->get_name()) + ": concat_status"; + diag_status_msg.hardware_id = "concatenate_data_checker"; + for (const std::string & e : input_topics_) { - const std::string subscribe_status = not_subscribed_topic_names_.count(e) ? "NG" : "OK"; - stat.add(e, subscribe_status); + diagnostic_msgs::msg::KeyValue key_value_msg; + key_value_msg.key = e; + key_value_msg.value = not_subscribed_topic_names_.count(e) ? "NG" : "OK"; + diag_status_msg.values.push_back(key_value_msg); + } + + if (not_subscribed_topic_names_.size() > 0) { + consecutive_concatenate_failures += 1; + } else { + consecutive_concatenate_failures = 0; } - const int8_t level = not_subscribed_topic_names_.empty() - ? diagnostic_msgs::msg::DiagnosticStatus::OK - : diagnostic_msgs::msg::DiagnosticStatus::WARN; - const std::string message = not_subscribed_topic_names_.empty() - ? "Concatenate all topics" - : "Some topics are not concatenated"; - stat.summary(level, message); + { + diagnostic_msgs::msg::KeyValue key_value_msg; + key_value_msg.key = "consecutiveConcatenateFailures"; + key_value_msg.value = std::to_string(consecutive_concatenate_failures); + diag_status_msg.values.push_back(key_value_msg); + } + + if (consecutive_concatenate_failures > 1) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message = "Some topics are not concatenated"; + } else { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diag_status_msg.message = "Concatenate all topics"; + } + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_status_msg); + + diagnostics_pub_->publish(diag_msg); } } // namespace pointcloud_preprocessor