Add in timestamping for usability (#4)

* Add in timestamping for usability

* Switch to promise future instead of sleeps

Author: zeerekahmad

socketcan_adapter/include/socketcan_adapter/can_frame.hpp

@@ -18,6 +18,7 @@
 #include <linux/can.h>
 
 #include <array>
+#include <chrono>
 #include <cstdint>
 #include <memory>
 #include <string>
@@ -62,7 +63,7 @@ class CanFrame
   CanFrame(
     const canid_t raw_id,
     const std::array<unsigned char, CAN_MAX_DLC> & data,
-    const uint64_t & timestamp,
+    const uint64_t & bus_timestamp,
     uint8_t len = CAN_MAX_DLC);
 
   /// @brief Initialize CanFrame with a partial ID and data defined in std::array
@@ -74,7 +75,7 @@ class CanFrame
   CanFrame(
     const canid_t id,
     const std::array<unsigned char, CAN_MAX_DLC> & data,
-    const uint64_t & timestamp,
+    const uint64_t & bus_timestamp,
     FrameType & frame_type,
     IdType & frame_id_type,
     uint8_t len = CAN_MAX_DLC);
@@ -113,9 +114,29 @@ class CanFrame
   /// @param data INPUT raw data to pass through the socket
   void set_data(const std::array<unsigned char, CAN_MAX_DLC> & data);
 
-  /// @brief Set timestamp
+  /// @brief Set bus timestamp
+  /// @param timestamp INPUT time as uint64_t from the bus
+  void set_bus_timestamp(const uint64_t & timestamp);
+
+  /// @brief Set receive timestamp
   /// @param timestamp INPUT time as uint64_t from the bus
-  void set_timestamp(const uint64_t & timestamp);
+  void set_receive_timestamp(const uint64_t & timestamp);
+
+  /// @brief Set timestamp
+  /// @param timestamp INPUT time as std::chrono::system_clock::time_point from the bus
+  void set_bus_timestamp(const std::chrono::system_clock::time_point & timestamp);
+
+  /// @brief Set timestamp
+  /// @param timestamp INPUT time as std::chrono::steady_clock::time_point from the adapter
+  void set_receive_timestamp(const std::chrono::steady_clock::time_point & timestamp);
+
+  /// @brief Get bus time
+  /// @return returns std::chrono::system_clock::time_point for when the frame was received on the bus
+  std::chrono::system_clock::time_point get_bus_time() const;
+
+  /// @brief Get receive time
+  /// @return returns std::chrono::steady_clock::time_point for when the frame was received by the adapter
+  std::chrono::steady_clock::time_point get_receive_time() const;
 
   /// @brief Get frame type
   /// @return returns polymath::socketcan::FrameType
@@ -156,7 +177,8 @@ class CanFrame
 
 private:
   struct can_frame frame_{};
-  uint64_t timestamp_{};
+  std::chrono::steady_clock::time_point receive_time_{};
+  std::chrono::system_clock::time_point bus_time_{};
 };
 
 }  // namespace polymath::socketcan

socketcan_adapter/src/can_frame.cpp

@@ -32,8 +32,12 @@ CanFrame::CanFrame(const struct can_frame & frame)
 {}
 
 CanFrame::CanFrame(
-  const canid_t raw_id, const std::array<unsigned char, CAN_MAX_DLC> & data, const uint64_t & timestamp, uint8_t len)
-: timestamp_(timestamp)
+  const canid_t raw_id,
+  const std::array<unsigned char, CAN_MAX_DLC> & data,
+  const uint64_t & bus_timestamp,
+  uint8_t len)
+: receive_time_(std::chrono::steady_clock::now())
+, bus_time_(std::chrono::system_clock::time_point(std::chrono::microseconds(bus_timestamp)))
 {
   set_can_id(raw_id);
   std::copy(data.begin(), data.end(), frame_.data);
@@ -43,11 +47,12 @@ CanFrame::CanFrame(
 CanFrame::CanFrame(
   const canid_t id,
   const std::array<unsigned char, CAN_MAX_DLC> & data,
-  const uint64_t & timestamp,
+  const uint64_t & bus_timestamp,
   FrameType & frame_type,
   IdType & frame_id_type,
   uint8_t len)
-: timestamp_(timestamp)
+: receive_time_(std::chrono::steady_clock::now())
+, bus_time_(std::chrono::system_clock::time_point(std::chrono::microseconds(bus_timestamp)))
 {
   set_can_id(id);
   set_data(data);
@@ -130,9 +135,34 @@ void CanFrame::set_data(const std::array<unsigned char, CAN_MAX_DLC> & data)
   std::copy(data.begin(), data.end(), frame_.data);
 }
 
-void CanFrame::set_timestamp(const uint64_t & timestamp)
+void CanFrame::set_bus_timestamp(const uint64_t & timestamp)
+{
+  bus_time_ = std::chrono::system_clock::time_point(std::chrono::microseconds(timestamp));
+}
+
+void CanFrame::set_receive_timestamp(const uint64_t & timestamp)
+{
+  receive_time_ = std::chrono::steady_clock::time_point(std::chrono::microseconds(timestamp));
+}
+
+void CanFrame::set_bus_timestamp(const std::chrono::system_clock::time_point & timestamp)
+{
+  bus_time_ = timestamp;
+}
+
+void CanFrame::set_receive_timestamp(const std::chrono::steady_clock::time_point & timestamp)
+{
+  receive_time_ = timestamp;
+}
+
+std::chrono::system_clock::time_point CanFrame::get_bus_time() const
+{
+  return bus_time_;
+}
+
+std::chrono::steady_clock::time_point CanFrame::get_receive_time() const
 {
-  timestamp_ = timestamp;
+  return receive_time_;
 }
 
 IdType CanFrame::get_id_type() const

socketcan_adapter/src/socketcan_adapter.cpp

@@ -24,6 +24,7 @@
 #include <unistd.h>
 
 #include <cerrno>
+#include <chrono>
 #include <cstring>
 #include <future>
 #include <memory>
@@ -172,9 +173,12 @@ std::optional<SocketcanAdapter::socket_error_string_t> SocketcanAdapter::receive
     struct timeval tv;
     ioctl(socket_file_descriptor_, SIOCGSTAMP, &tv);
 
-    uint64_t timestamp_uint64 = static_cast<uint64_t>(tv.tv_sec) * 1e6 + tv.tv_usec;
+    // uint64_t timestamp_uint64 = static_cast<uint64_t>(tv.tv_sec) * 1e6 + tv.tv_usec;
+    std::chrono::system_clock::time_point timestamp{
+      std::chrono::seconds{tv.tv_sec} + std::chrono::microseconds{tv.tv_usec}};
     polymath_can_frame.set_frame(frame);
-    polymath_can_frame.set_timestamp(timestamp_uint64);
+    polymath_can_frame.set_bus_timestamp(timestamp);
+    polymath_can_frame.set_receive_timestamp(std::chrono::steady_clock::now());
   }
 
   return error_string.empty() ? std::nullopt : std::optional<socket_error_string_t>(error_string);

socketcan_adapter/test/can_frame_test.cpp

@@ -14,6 +14,7 @@
 
 #include <algorithm>
 #include <array>
+#include <chrono>
 #include <cstdint>
 #include <string>
 
@@ -168,3 +169,103 @@ TEST_CASE("Get error as string", "[CanFrame]")
   std::string error = frame.get_error();
   REQUIRE(!error.empty());
 }
+
+TEST_CASE("Set and get bus timestamp with uint64_t", "[CanFrame]")
+{
+  polymath::socketcan::CanFrame frame;
+  std::uint64_t timestamp_us = 1000000;  // 1 second in microseconds
+
+  frame.set_bus_timestamp(timestamp_us);
+  auto bus_time = frame.get_bus_time();
+
+  auto expected = std::chrono::system_clock::time_point(std::chrono::microseconds(timestamp_us));
+  REQUIRE(bus_time == expected);
+}
+
+TEST_CASE("Set and get receive timestamp with uint64_t", "[CanFrame]")
+{
+  polymath::socketcan::CanFrame frame;
+  std::uint64_t timestamp_us = 2000000;  // 2 seconds in microseconds
+
+  frame.set_receive_timestamp(timestamp_us);
+  auto receive_time = frame.get_receive_time();
+
+  auto expected = std::chrono::steady_clock::time_point(std::chrono::microseconds(timestamp_us));
+  REQUIRE(receive_time == expected);
+}
+
+TEST_CASE("Set and get bus timestamp with time_point", "[CanFrame]")
+{
+  polymath::socketcan::CanFrame frame;
+  auto timestamp = std::chrono::system_clock::now();
+
+  frame.set_bus_timestamp(timestamp);
+  auto bus_time = frame.get_bus_time();
+
+  REQUIRE(bus_time == timestamp);
+}
+
+TEST_CASE("Set and get receive timestamp with time_point", "[CanFrame]")
+{
+  polymath::socketcan::CanFrame frame;
+  auto timestamp = std::chrono::steady_clock::now();
+
+  frame.set_receive_timestamp(timestamp);
+  auto receive_time = frame.get_receive_time();
+
+  REQUIRE(receive_time == timestamp);
+}
+
+TEST_CASE("Constructor initializes receive_time to now", "[CanFrame]")
+{
+  auto before = std::chrono::steady_clock::now();
+
+  canid_t raw_id = 0x123;
+  std::array<unsigned char, CAN_MAX_DLC> data = {1, 2, 3, 4, 5, 6, 7, 8};
+  std::uint64_t bus_timestamp = 123456789;
+  polymath::socketcan::CanFrame frame(raw_id, data, bus_timestamp);
+
+  auto after = std::chrono::steady_clock::now();
+  auto receive_time = frame.get_receive_time();
+
+  REQUIRE(receive_time >= before);
+  REQUIRE(receive_time <= after);
+}
+
+TEST_CASE("Constructor sets bus_time from timestamp parameter", "[CanFrame]")
+{
+  canid_t raw_id = 0x123;
+  std::array<unsigned char, CAN_MAX_DLC> data = {1, 2, 3, 4, 5, 6, 7, 8};
+  std::uint64_t bus_timestamp = 5000000;  // 5 seconds in microseconds
+
+  polymath::socketcan::CanFrame frame(raw_id, data, bus_timestamp);
+  auto bus_time = frame.get_bus_time();
+
+  auto expected = std::chrono::system_clock::time_point(std::chrono::microseconds(bus_timestamp));
+  REQUIRE(bus_time == expected);
+}
+
+TEST_CASE("Extended constructor initializes timestamps correctly", "[CanFrame]")
+{
+  auto before = std::chrono::steady_clock::now();
+
+  canid_t raw_id = 0x789;
+  std::array<unsigned char, CAN_MAX_DLC> data = {9, 8, 7, 6, 5, 4, 3, 2};
+  std::uint64_t bus_timestamp = 10000000;  // 10 seconds in microseconds
+  auto frame_type = polymath::socketcan::FrameType::REMOTE;
+  auto frame_id_type = polymath::socketcan::IdType::EXTENDED;
+
+  polymath::socketcan::CanFrame frame(raw_id, data, bus_timestamp, frame_type, frame_id_type);
+
+  auto after = std::chrono::steady_clock::now();
+
+  // Check receive_time was set to approximately now
+  auto receive_time = frame.get_receive_time();
+  REQUIRE(receive_time >= before);
+  REQUIRE(receive_time <= after);
+
+  // Check bus_time was set from the parameter
+  auto bus_time = frame.get_bus_time();
+  auto expected_bus_time = std::chrono::system_clock::time_point(std::chrono::microseconds(bus_timestamp));
+  REQUIRE(bus_time == expected_bus_time);
+}