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; }