From df72a5e40bd8697104c02cbb7deab815a7813a3c Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko 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 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 lock(cam_mutex[cam_id]); #if CV_MAJOR_VERSION >= 3 cam_data[cam_id] << img->t_ns << "," << img->t_ns << ".webp" << std::endl;