From 33f177264159f09e2f23a84aa8473eb6e729b7c3 Mon Sep 17 00:00:00 2001
From: Nicolas Jacquemin <nicolas.jacquemin@ams.com>
Date: Mon, 8 Jan 2024 09:17:57 +0100
Subject: [PATCH] Keep vrpn timestamps in vrpn timebase

---
 include/vrpn_mocap/tracker.hpp |  3 ---
 src/tracker.cpp                | 18 ++----------------
 2 files changed, 2 insertions(+), 19 deletions(-)

diff --git a/include/vrpn_mocap/tracker.hpp b/include/vrpn_mocap/tracker.hpp
index 712b21e..8d831fe 100644
--- a/include/vrpn_mocap/tracker.hpp
+++ b/include/vrpn_mocap/tracker.hpp
@@ -104,9 +104,6 @@ class Tracker : public rclcpp::Node
   const bool use_vrpn_timestamps_;
   const std::shared_ptr<vrpn_Connection> connection_;
 
-  builtin_interfaces::msg::Time first_ros_timestamp_;
-  struct timeval first_vrpn_timestamp_;
-
   vrpn_Tracker_Remote vrpn_tracker_;
 
   std::vector<PublisherT<geometry_msgs::msg::PoseStamped>::SharedPtr> pose_pubs_;
diff --git a/src/tracker.cpp b/src/tracker.cpp
index 89db08f..0c0d638 100644
--- a/src/tracker.cpp
+++ b/src/tracker.cpp
@@ -100,23 +100,9 @@ void Tracker::MainLoop() {vrpn_tracker_.mainloop();}
 
 builtin_interfaces::msg::Time Tracker::get_timestamp(struct timeval vrpn_timestamp) {
   if (this->use_vrpn_timestamps_) {
-    if (this->first_ros_timestamp_.sec == 0) {
-      this->first_ros_timestamp_ = this->get_clock()->now();
-      this->first_vrpn_timestamp_ = vrpn_timestamp;
-    }
     builtin_interfaces::msg::Time stamp;
-    int64_t sec = vrpn_timestamp.tv_sec - this->first_vrpn_timestamp_.tv_sec + this->first_ros_timestamp_.sec;
-    int64_t nanosec = (vrpn_timestamp.tv_usec - this->first_vrpn_timestamp_.tv_usec) * 1000 + this->first_ros_timestamp_.nanosec;
-    while (nanosec < 0) {
-      nanosec += 1e9;
-      sec -= 1;
-    }
-    while (nanosec >= 1e9) {
-      nanosec -= 1e9;
-      sec += 1;
-    }
-    stamp.sec = sec;
-    stamp.nanosec = nanosec;
+    stamp.sec = vrpn_timestamp.tv_sec;
+    stamp.nanosec = vrpn_timestamp.tv_usec * 1000;
 
     return stamp;
   }