From 336bf65679c4e7c07f04a39c61b5069d38ebac1c Mon Sep 17 00:00:00 2001 From: mfaferek93 Date: Sun, 1 Feb 2026 17:59:40 +0000 Subject: [PATCH] fix(fault_manager): #155 use wall clock time instead of simulation time When running with use_sim_time=true, fault timestamps were recorded in simulation time (seconds since sim start) instead of wall clock time. This caused incorrect timestamps like "1970-01-01" when UI interpreted simulation time as Unix epoch. Changes: - Add time_utils.hpp with get_wall_clock_ns() and get_wall_clock_time() - Replace node->now() with wall clock utilities for fault timestamps in: - FaultManagerNode: report_fault_event, publish_fault_event, auto-confirm - SnapshotCapture: on-demand and background cache timestamps - RosbagCapture: message timestamps and bag metadata - Add unit test to verify timestamps are valid wall clock values --- .../ros2_medkit_fault_manager/time_utils.hpp | 36 +++++++++++++++++++ .../src/fault_manager_node.cpp | 9 ++--- .../src/rosbag_capture.cpp | 13 ++++--- .../src/snapshot_capture.cpp | 9 ++--- .../test/test_fault_manager.cpp | 28 +++++++++++++++ 5 files changed, 82 insertions(+), 13 deletions(-) create mode 100644 src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/time_utils.hpp diff --git a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/time_utils.hpp b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/time_utils.hpp new file mode 100644 index 00000000..0146a53b --- /dev/null +++ b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/time_utils.hpp @@ -0,0 +1,36 @@ +// Copyright 2026 mfaferek93 +// +// 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. + +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" + +namespace ros2_medkit_fault_manager { + +/// Get current wall clock time in nanoseconds. +/// This is not affected by use_sim_time parameter. +inline int64_t get_wall_clock_ns() { + auto now = std::chrono::system_clock::now(); + return std::chrono::duration_cast(now.time_since_epoch()).count(); +} + +/// Get current wall clock time as rclcpp::Time. +/// This is not affected by use_sim_time parameter. +inline rclcpp::Time get_wall_clock_time() { + return rclcpp::Time(get_wall_clock_ns(), RCL_SYSTEM_TIME); +} + +} // namespace ros2_medkit_fault_manager diff --git a/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp b/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp index d89bfedd..f215d8e4 100644 --- a/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp +++ b/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp @@ -24,6 +24,7 @@ #include "ros2_medkit_fault_manager/correlation/config_parser.hpp" #include "ros2_medkit_fault_manager/sqlite_fault_storage.hpp" +#include "ros2_medkit_fault_manager/time_utils.hpp" #include "ros2_medkit_msgs/msg/cluster_info.hpp" #include "ros2_medkit_msgs/msg/muted_fault_info.hpp" @@ -172,7 +173,7 @@ FaultManagerNode::FaultManagerNode(const rclcpp::NodeOptions & options) : Node(" // Create auto-confirmation timer if enabled if (auto_confirm_after_sec_ > 0.0) { auto_confirm_timer_ = create_wall_timer(std::chrono::seconds(1), [this]() { - size_t confirmed = storage_->check_time_based_confirmation(now()); + size_t confirmed = storage_->check_time_based_confirmation(get_wall_clock_time()); if (confirmed > 0) { RCLCPP_INFO(get_logger(), "Auto-confirmed %zu PREFAILED fault(s) due to time threshold", confirmed); } @@ -258,9 +259,9 @@ void FaultManagerNode::handle_report_fault( auto fault_before = storage_->get_fault(request->fault_code); std::string status_before = fault_before ? fault_before->status : ""; - // Report the fault event + // Report the fault event (use wall clock time, not sim time, for proper timestamps) bool is_new = storage_->report_fault_event(request->fault_code, request->event_type, request->severity, - request->description, request->source_id, now()); + request->description, request->source_id, get_wall_clock_time()); response->accepted = true; @@ -471,7 +472,7 @@ void FaultManagerNode::publish_fault_event(const std::string & event_type, const ros2_medkit_msgs::msg::FaultEvent event; event.event_type = event_type; event.fault = fault; - event.timestamp = now(); + event.timestamp = get_wall_clock_time(); event.auto_cleared_codes = auto_cleared_codes; event_publisher_->publish(event); diff --git a/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp b/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp index 812a94f4..5f980168 100644 --- a/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp +++ b/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp @@ -18,13 +18,14 @@ #include #include -#include #include #include #include #include #include +#include "ros2_medkit_fault_manager/time_utils.hpp" + namespace ros2_medkit_fault_manager { namespace { @@ -213,7 +214,7 @@ void RosbagCapture::on_fault_confirmed(const std::string & fault_code) { info.format = config_.format; info.duration_sec = config_.duration_sec; info.size_bytes = bag_size; - info.created_at_ns = node_->now().nanoseconds(); + info.created_at_ns = get_wall_clock_ns(); storage_->store_rosbag_file(info); enforce_storage_limits(); @@ -336,7 +337,8 @@ void RosbagCapture::message_callback(const std::string & topic, const std::strin return; } - int64_t timestamp_ns = node_->now().nanoseconds(); + // Use wall clock time, not sim time, for proper timestamps + int64_t timestamp_ns = get_wall_clock_ns(); // During post-fault recording, write directly to bag (no buffering) if (recording_post_fault_.load()) { @@ -394,7 +396,8 @@ void RosbagCapture::prune_buffer() { return; } - int64_t now_ns = node_->now().nanoseconds(); + // Use wall clock time, not sim time, for consistent buffer pruning + int64_t now_ns = get_wall_clock_ns(); int64_t duration_ns = static_cast(config_.duration_sec * 1e9); int64_t cutoff_ns = now_ns - duration_ns; @@ -651,7 +654,7 @@ void RosbagCapture::post_fault_timer_callback() { info.format = config_.format; info.duration_sec = config_.duration_sec + config_.duration_after_sec; info.size_bytes = bag_size; - info.created_at_ns = node_->now().nanoseconds(); + info.created_at_ns = get_wall_clock_ns(); storage_->store_rosbag_file(info); enforce_storage_limits(); diff --git a/src/ros2_medkit_fault_manager/src/snapshot_capture.cpp b/src/ros2_medkit_fault_manager/src/snapshot_capture.cpp index 4431f2b8..43d32c69 100644 --- a/src/ros2_medkit_fault_manager/src/snapshot_capture.cpp +++ b/src/ros2_medkit_fault_manager/src/snapshot_capture.cpp @@ -14,7 +14,6 @@ #include "ros2_medkit_fault_manager/snapshot_capture.hpp" -#include #include #include @@ -22,6 +21,7 @@ #include #include +#include "ros2_medkit_fault_manager/time_utils.hpp" #include "ros2_medkit_serialization/json_serializer.hpp" #include "ros2_medkit_serialization/serialization_error.hpp" @@ -213,13 +213,13 @@ bool SnapshotCapture::capture_topic_on_demand(const std::string & fault_code, co ros2_medkit_serialization::JsonSerializer serializer; auto json_data = serializer.deserialize(msg_type, captured_msg); - // Store snapshot + // Store snapshot (use wall clock time, not sim time, for proper timestamps) SnapshotData snapshot; snapshot.fault_code = fault_code; snapshot.topic = topic; snapshot.message_type = msg_type; snapshot.data = json_data.dump(); - snapshot.captured_at_ns = node_->now().nanoseconds(); + snapshot.captured_at_ns = get_wall_clock_ns(); storage_->store_snapshot(snapshot); @@ -299,7 +299,8 @@ void SnapshotCapture::init_background_subscriptions() { cached.topic = topic; cached.message_type = msg_type; cached.data = json_data.dump(); - cached.timestamp_ns = node_->now().nanoseconds(); + // Use wall clock time, not sim time, for proper timestamps + cached.timestamp_ns = get_wall_clock_ns(); } catch (const std::exception & e) { RCLCPP_DEBUG(node_->get_logger(), "Failed to cache message from '%s': %s", topic.c_str(), e.what()); diff --git a/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp b/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp index 22714a48..a6932606 100644 --- a/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp +++ b/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp @@ -859,6 +859,34 @@ TEST_F(FaultEventPublishingTest, EventContainsFullFaultData) { EXPECT_EQ(fault.reporting_sources[0], "/sensor/temperature"); } +TEST_F(FaultEventPublishingTest, TimestampUsesWallClockNotSimTime) { + // Get current wall clock time + auto wall_before = std::chrono::system_clock::now(); + auto wall_before_ns = std::chrono::duration_cast(wall_before.time_since_epoch()).count(); + + ASSERT_TRUE(call_report_fault("WALL_CLOCK_TEST", Fault::SEVERITY_WARN, "/test_node")); + spin_for(std::chrono::milliseconds(100)); + + auto wall_after = std::chrono::system_clock::now(); + auto wall_after_ns = std::chrono::duration_cast(wall_after.time_since_epoch()).count(); + + ASSERT_EQ(received_events_.size(), 1u); + + // Convert event timestamp to nanoseconds + rclcpp::Time event_time(received_events_[0].timestamp); + int64_t event_ns = event_time.nanoseconds(); + + // Verify timestamp is close to wall clock time (within 5 seconds to be safe) + // This catches the bug where sim time (e.g., 148 seconds) would be far from wall clock + EXPECT_GE(event_ns, wall_before_ns - 5'000'000'000LL); + EXPECT_LE(event_ns, wall_after_ns + 5'000'000'000LL); + + // Also verify it's a reasonable Unix timestamp (after year 2020) + // This catches the "1970" bug from the issue + constexpr int64_t YEAR_2020_NS = 1577836800LL * 1'000'000'000LL; // 2020-01-01 00:00:00 UTC + EXPECT_GT(event_ns, YEAR_2020_NS); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); ::testing::InitGoogleTest(&argc, argv);