From df72a5e40bd8697104c02cbb7deab815a7813a3c Mon Sep 17 00:00:00 2001
From: Vladyslav Usenko <vlad.usenko@tum.de>
Date: Fri, 28 Jun 2019 15:43:19 +0000
Subject: [PATCH] realsense

---
 scripts/capture_mocap.py | 47 ++++++++++++++++++++++++++++++++++++++++
 src/rs_t265_record.cpp   |  2 ++
 2 files changed, 49 insertions(+)
 create mode 100755 scripts/capture_mocap.py

diff --git a/scripts/capture_mocap.py b/scripts/capture_mocap.py
new file mode 100755
index 0000000..d66ebc0
--- /dev/null
+++ b/scripts/capture_mocap.py
@@ -0,0 +1,47 @@
+#!/usr/bin/env python
+
+import sys
+import os
+import rospy
+from geometry_msgs.msg import TransformStamped
+
+
+def callback(data):
+    global out_file, time_offset
+    if not out_file:
+        return
+
+    if not time_offset:
+        time_offset = rospy.Time().now() - data.header.stamp
+
+    out_file.write('{},{},{},{},{},{},{},{}\n'.format(
+        data.header.stamp + time_offset,
+        data.transform.translation.x,
+        data.transform.translation.y,
+        data.transform.translation.z,
+        data.transform.rotation.w,
+        data.transform.rotation.x,
+        data.transform.rotation.y,
+        data.transform.rotation.z
+    ))
+
+
+def listener():
+    rospy.init_node('listener', anonymous=True)
+    rospy.Subscriber('/vrpn_client/raw_transform', TransformStamped, callback)
+
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    out_file = None
+    time_offset = None
+
+    if not os.path.exists(sys.argv[1]):
+        os.makedirs(sys.argv[1])
+
+    out_file = open(sys.argv[1] + '/data.csv', 'w')
+    out_file.write('#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []\n')
+    listener()
+    out_file.close()
+
diff --git a/src/rs_t265_record.cpp b/src/rs_t265_record.cpp
index 0f4b588..04f7390 100644
--- a/src/rs_t265_record.cpp
+++ b/src/rs_t265_record.cpp
@@ -85,6 +85,7 @@ static constexpr int NUM_CAMS = basalt::RsT265Device::NUM_CAMS;
 static constexpr int NUM_WORKERS = 8;
 
 std::ofstream cam_data[NUM_CAMS], exposure_data[NUM_CAMS], imu0_data, pose_data;
+std::mutex cam_mutex[NUM_CAMS];
 
 std::vector<std::thread> worker_threads;
 std::thread imu_worker_thread, pose_worker_thread;
@@ -99,6 +100,7 @@ void image_save_worker() {
     if (image_data_queue.try_pop(img)) {
       if (recording) {
         for (size_t cam_id = 0; cam_id < NUM_CAMS; ++cam_id) {
+          std::scoped_lock<std::mutex> lock(cam_mutex[cam_id]);
 #if CV_MAJOR_VERSION >= 3
           cam_data[cam_id] << img->t_ns << "," << img->t_ns << ".webp"
                            << std::endl;