Add option to use VRPN timestamps rather than generating them again (#7)

* Add option to use VRPN timestamps rather than generating them again

* Correctly handle seconds and nanoseconds

* Keep vrpn timestamps in vrpn timebase

* Switch to CamelCase for consistency

* Fix style

README.md

@@ -55,6 +55,7 @@ file with custom configurations.
   system default QoS which is reliable (default: `true`)
 - `multi_sensor (bool)` -- set to true if there are more than one sensor (frame) reporting on
   the same object (default: `false`)
+- `use_vrpn_timestamps (bool)` -- use timestamps coming from VRPN. This ensures that the interval between frames timestamps is not modified (default: `false`)
 
 ### Acknowledgement
 Some ideas are borrowed from the well known

config/client.yaml

@@ -6,3 +6,4 @@
     update_freq: 100.
     refresh_freq: 1.
     multi_sensor: false
+    use_vrpn_timestamps: false

include/vrpn_mocap/tracker.hpp

@@ -31,6 +31,7 @@
 #include "vrpn_Connection.h"
 #include "vrpn_Tracker.h"
 
+#include "builtin_interfaces/msg/time.hpp"
 #include "geometry_msgs/msg/accel_stamped.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
@@ -100,6 +101,7 @@ class Tracker : public rclcpp::Node
   const bool multi_sensor_;
   const std::string frame_id_;
   const bool sensor_data_qos_;
+  const bool use_vrpn_timestamps_;
   const std::shared_ptr<vrpn_Connection> connection_;
 
   vrpn_Tracker_Remote vrpn_tracker_;
@@ -139,6 +141,8 @@ class Tracker : public rclcpp::Node
     return pubs->at(sensor_idx);
   }
 
+  builtin_interfaces::msg::Time GetTimestamp(struct timeval vrpn_timestamp);
+
   static void VRPN_CALLBACK HandlePose(void * tracker, const vrpn_TRACKERCB tracker_pose);
   static void VRPN_CALLBACK HandleTwist(void * tracker, const vrpn_TRACKERVELCB tracker_vel);
   static void VRPN_CALLBACK HandleAccel(void * tracker, const vrpn_TRACKERACCCB tracker_acc);

src/client.cpp

@@ -47,6 +47,7 @@ Client::Client(const std::string & name)
   mainloop_timer_ = this->create_wall_timer(1s / update_freq, std::bind(&Client::MainLoop, this));
 
   this->declare_parameter("sensor_data_qos", true);
+  this->declare_parameter("use_vrpn_timestamps", false);
 }
 
 std::string Client::ParseHost()

src/tracker.cpp

@@ -53,6 +53,7 @@ Tracker::Tracker(const std::string & tracker_name)
   multi_sensor_(declare_parameter("multi_sensor", false)),
   frame_id_(declare_parameter("frame_id", "world")),
   sensor_data_qos_(declare_parameter("sensor_data_qos", true)),
+  use_vrpn_timestamps_(declare_parameter("use_vrpn_timestamps", false)),
   vrpn_tracker_(name_.c_str())
 {
   Init();
@@ -70,6 +71,7 @@ Tracker::Tracker(
   multi_sensor_(base_node.get_parameter("multi_sensor").as_bool()),
   frame_id_(base_node.get_parameter("frame_id").as_string()),
   sensor_data_qos_(base_node.get_parameter("sensor_data_qos").as_bool()),
+  use_vrpn_timestamps_(base_node.get_parameter("use_vrpn_timestamps").as_bool()),
   vrpn_tracker_(name_.c_str(), connection.get())
 {
   Init();
@@ -96,6 +98,18 @@ void Tracker::Init()
 
 void Tracker::MainLoop() {vrpn_tracker_.mainloop();}
 
+builtin_interfaces::msg::Time Tracker::GetTimestamp(struct timeval vrpn_timestamp)
+{
+  if (this->use_vrpn_timestamps_) {
+    builtin_interfaces::msg::Time stamp;
+    stamp.sec = vrpn_timestamp.tv_sec;
+    stamp.nanosec = vrpn_timestamp.tv_usec * 1000;
+
+    return stamp;
+  }
+  return this->get_clock()->now();
+}
+
 void VRPN_CALLBACK Tracker::HandlePose(void * data, const vrpn_TRACKERCB tracker_pose)
 {
   Tracker * tracker = static_cast<Tracker *>(data);
@@ -107,7 +121,7 @@ void VRPN_CALLBACK Tracker::HandlePose(void * data, const vrpn_TRACKERCB tracker
   // populate message
   PoseStamped msg;
   msg.header.frame_id = tracker->frame_id_;
-  msg.header.stamp = tracker->get_clock()->now();
+  msg.header.stamp = tracker->GetTimestamp(tracker_pose.msg_time);
 
   msg.pose.position.x = tracker_pose.pos[0];
   msg.pose.position.y = tracker_pose.pos[1];
@@ -132,7 +146,7 @@ void VRPN_CALLBACK Tracker::HandleTwist(void * data, const vrpn_TRACKERVELCB tra
   // populate message
   TwistStamped msg;
   msg.header.frame_id = tracker->frame_id_;
-  msg.header.stamp = tracker->get_clock()->now();
+  msg.header.stamp = tracker->GetTimestamp(tracker_twist.msg_time);
 
   msg.twist.linear.x = tracker_twist.vel[0];
   msg.twist.linear.y = tracker_twist.vel[1];
@@ -161,7 +175,7 @@ void VRPN_CALLBACK Tracker::HandleAccel(void * data, const vrpn_TRACKERACCCB tra
   // populate message
   AccelStamped msg;
   msg.header.frame_id = tracker->frame_id_;
-  msg.header.stamp = tracker->get_clock()->now();
+  msg.header.stamp = tracker->GetTimestamp(tracker_accel.msg_time);
 
   msg.accel.linear.x = tracker_accel.acc[0];
   msg.accel.linear.y = tracker_accel.acc[1];