Synchronize timestamps to unix time

We now correct the local system/unix time such that the timestamps
point to the mid-exposure time in unix time. This addresses
issue #9, however with the new NatNetSDK update there might be more
changes.

clients/ros2_client.hpp

@@ -61,8 +61,16 @@ class NatNet2Ros2 : public CyberZooMocapClient, public rclcpp::Node
                 // Get the current pose
                 pose_t pose = this->getPoseRB(i);
 
+                // Init Message
                 geometry_msgs::msg::PoseStamped msg{};
-                msg.header.stamp = this->now();
+
+                // Transform the pose timestamp to unix time
+                double seconds = this->seconds_since_mocap_ts(pose.timeUs);
+                uint32_t seconds_t = static_cast<int32_t>(seconds);
+                uint32_t nanoseconds_t = static_cast<int32_t>((seconds - seconds_t) * 1e9);
+                msg.header.stamp = (this->now() - rclcpp::Duration(seconds_t, nanoseconds_t));
+
+                // Init Frame ID
                 msg.header.frame_id = "world";
 
                 // Save in message
@@ -75,6 +83,7 @@ class NatNet2Ros2 : public CyberZooMocapClient, public rclcpp::Node
                 msg.pose.orientation.y = pose.qy;
                 msg.pose.orientation.z = pose.qz;
 
+                // Finally publish
                 this->_publishers.at(i)->publish(msg);
             }
         }

include/cyberzoo_mocap_client.hpp

@@ -119,6 +119,9 @@ class CyberZooMocapClient
         return pose_der;
     };
 
+    /* Returns the seconds since the a timestamp in
+     * MoCap reference time */
+    double seconds_since_mocap_ts(uint64_t us);
     /* Function that is used to spin up the publish thread */
     void publish_loop();
     void keystroke_loop();

src/cyberzoo_mocap_client.cpp

@@ -138,6 +138,11 @@ void CyberZooMocapClient::parse_extra_po(const boost::program_options::variables
     (void)vm;
 }
 
+double CyberZooMocapClient::seconds_since_mocap_ts(uint64_t us)
+{
+   return this->pClient->SecondsSinceHostTimestamp(us);
+}
+
 void CyberZooMocapClient::publish_loop()
 {
     bool run = true;