fix diag time stamp

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>
autowarefoundation · May 31, 2024 · 756359b · 756359b
1 parent dd1423b
commit 756359b
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 8 deletions.
diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp
@@ -34,10 +34,10 @@ class DiagnosticsModule
   template <typename T>
   void addKeyValue(const std::string & key, const T & value);
   void updateLevelAndMessage(const int8_t level, const std::string & message);
-  void publish();
+  void publish(const rclcpp::Time & publish_time_stamp);
 
 private:
-  diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray() const;
+  diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray(const rclcpp::Time & publish_time_stamp) const;
 
   rclcpp::Clock::SharedPtr clock_;
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;

diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp
@@ -88,15 +88,15 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str
   }
 }
 
-void DiagnosticsModule::publish()
+void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp)
 {
-  diagnostics_pub_->publish(createDiagnosticsArray());
+  diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp));
 }
 
-diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray() const
+diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray(const rclcpp::Time & publish_time_stamp) const
 {
   diagnostic_msgs::msg::DiagnosticArray diagnostics_msg;
-  diagnostics_msg.header.stamp = clock_->now();
+  diagnostics_msg.header.stamp = publish_time_stamp;
   diagnostics_msg.status.push_back(diagnostics_status_msg_);
 
   if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) {

diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp
@@ -91,7 +91,7 @@ void GyroOdometerNode::callbackVehicleTwist(
   vehicle_twist_queue_.push_back(*vehicle_twist_ptr);
   concatGyroAndOdometer();
 
-  diagnostics_->publish();
+  diagnostics_->publish(vehicle_twist_ptr->header.stamp);
 }
 
 
@@ -105,7 +105,7 @@ void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr i
   gyro_queue_.push_back(*imu_msg_ptr);
   concatGyroAndOdometer();
 
-  diagnostics_->publish();
+  diagnostics_->publish(imu_msg_ptr->header.stamp);
 }