Keep vrpn timestamps in vrpn timebase

alvinsunyixiao · Jan 8, 2024 · 33f1772 · 33f1772
1 parent a490d8d
commit 33f1772
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 19 deletions.
diff --git 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
@@ -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;
   }