add diagnostic

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>

localization/gyro_odometer/CMakeLists.txt

@@ -6,6 +6,7 @@ autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/gyro_odometer_core.cpp
+  src/diagnostics_module.cpp
 )
 
 target_link_libraries(${PROJECT_NAME} fmt)

localization/gyro_odometer/README.md

@@ -34,3 +34,18 @@
 - [Limitation] The frequency of the output messages depends on the frequency of the input IMU message.
 
 - [Limitation] We cannot produce reliable values for the lateral and vertical velocities. Therefore we assign large values to the corresponding elements in the output covariance matrix.
+
+## Diagnostics
+
+<img src="./media/diagnostic.png" alt="drawing" width="600"/>
+
+| Name                      | Description                                        | Transition condition to Warning | Transition condition to Error |
+| ------------------------- | -------------------------------------------------- | ------------------------------- | ----------------------------- |
+| `topic_time_stamp` | the time stamp of service calling. [nano second]                 | none                            | none                          |
+| `is_arrived_first_vehicle_twist`            | whether the vehicle twist topic has been received even once.  | not arrive yet                            | none                          |
+| `is_arrived_first_imu`      | whether the imu topic has been received even once.    | not arrive yet                            | none                          |
+| `vehicle_twist_time_stamp_dt`      |  the time difference between the current time and the latest vehicle twist topic. [second] | none                            |    the time is **longer** than `message_timeout_sec`                       |
+| `imu_time_stamp_dt`      |  the time difference between the current time and the latest imu topic. [second] | none                            |    the time is **longer** than `message_timeout_sec`                       |
+| `vehicle_twist_queue_size`      |  the size of vehicle_twist_queue.  | none                            |    none                       |
+| `imu_queue_size`      |  the size of gyro_queue.  | none                            |    none                       |
+| `is_succeed_transform_imu`      |  whether transform imu is succeed or not.  | none                            |    failed                       |

localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp

@@ -0,0 +1,64 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_
+#define GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <string>
+#include <vector>
+
+namespace autoware::gyro_odometer
+{
+
+class DiagnosticsModule
+{
+public:
+  DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name);
+  void clear();
+  void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg);
+  template <typename T>
+  void addKeyValue(const std::string & key, const T & value);
+  void updateLevelAndMessage(const int8_t level, const std::string & message);
+  void publish();
+
+private:
+  diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray() const;
+
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
+
+  diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_;
+};
+
+template <typename T>
+void DiagnosticsModule::addKeyValue(const std::string & key, const T & value)
+{
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = key;
+  key_value.value = std::to_string(value);
+  addKeyValue(key_value);
+}
+
+template <>
+void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value);
+template <>
+void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value);
+
+}  // namespace autoware::gyro_odometer
+
+#endif  // GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_

localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp

@@ -15,6 +15,8 @@
 #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
 #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
 
+#include "gyro_odometer/diagnostics_module.hpp"
+
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
 #include "tier4_autoware_utils/ros/msg_covariance.hpp"
 #include "tier4_autoware_utils/ros/transform_listener.hpp"
@@ -52,6 +54,7 @@ class GyroOdometerNode : public rclcpp::Node
   void callbackVehicleTwist(
     const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr);
   void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
+  void concatGyroAndOdometer();
   void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw);
 
   rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
@@ -74,8 +77,12 @@ class GyroOdometerNode : public rclcpp::Node
 
   bool vehicle_twist_arrived_;
   bool imu_arrived_;
+  rclcpp::Time latest_vehicle_twist_rostime_;
+  rclcpp::Time latest_imu_rostime_;
   std::deque<geometry_msgs::msg::TwistWithCovarianceStamped> vehicle_twist_queue_;
   std::deque<sensor_msgs::msg::Imu> gyro_queue_;
+
+  std::unique_ptr<DiagnosticsModule> diagnostics_;
 };
 
 }  // namespace autoware::gyro_odometer

localization/gyro_odometer/package.xml

@@ -18,6 +18,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>fmt</depend>
+  <depend>diagnostic_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

localization/gyro_odometer/src/diagnostics_module.cpp

@@ -0,0 +1,109 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "gyro_odometer/diagnostics_module.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <algorithm>
+#include <string>
+
+namespace autoware::gyro_odometer
+{
+
+DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name)
+: clock_(node->get_clock())
+{
+  diagnostics_pub_ =
+    node->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
+
+  diagnostics_status_msg_.name =
+    std::string(node->get_name()) + std::string(": ") + diagnostic_name;
+  diagnostics_status_msg_.hardware_id = node->get_name();
+}
+
+void DiagnosticsModule::clear()
+{
+  diagnostics_status_msg_.values.clear();
+  diagnostics_status_msg_.values.shrink_to_fit();
+
+  diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  diagnostics_status_msg_.message = "";
+}
+
+void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg)
+{
+  auto it = std::find_if(
+    std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values),
+    [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; });
+
+  if (it != std::cend(diagnostics_status_msg_.values)) {
+    it->value = key_value_msg.value;
+  } else {
+    diagnostics_status_msg_.values.push_back(key_value_msg);
+  }
+}
+
+template <>
+void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value)
+{
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = key;
+  key_value.value = value;
+  addKeyValue(key_value);
+}
+
+template <>
+void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value)
+{
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = key;
+  key_value.value = value ? "True" : "False";
+  addKeyValue(key_value);
+}
+
+void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message)
+{
+  if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
+    if (!diagnostics_status_msg_.message.empty()) {
+      diagnostics_status_msg_.message += "; ";
+    }
+    diagnostics_status_msg_.message += message;
+  }
+  if (level > diagnostics_status_msg_.level) {
+    diagnostics_status_msg_.level = level;
+  }
+}
+
+void DiagnosticsModule::publish()
+{
+  diagnostics_pub_->publish(createDiagnosticsArray());
+}
+
+diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray() const
+{
+  diagnostic_msgs::msg::DiagnosticArray diagnostics_msg;
+  diagnostics_msg.header.stamp = clock_->now();
+  diagnostics_msg.status.push_back(diagnostics_status_msg_);
+
+  if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
+    diagnostics_msg.status.at(0).message = "OK";
+  }
+
+  return diagnostics_msg;
+}
+
+}  // namespace autoware::gyro_odometer