realsense

This commit is contained in:
Vladyslav Usenko 2019-06-28 15:43:19 +00:00
parent d2cf8d323b
commit df72a5e40b
2 changed files with 49 additions and 0 deletions

47
scripts/capture_mocap.py Executable file
View File

@ -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()

View File

@ -85,6 +85,7 @@ static constexpr int NUM_CAMS = basalt::RsT265Device::NUM_CAMS;
static constexpr int NUM_WORKERS = 8; static constexpr int NUM_WORKERS = 8;
std::ofstream cam_data[NUM_CAMS], exposure_data[NUM_CAMS], imu0_data, pose_data; 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::vector<std::thread> worker_threads;
std::thread imu_worker_thread, pose_worker_thread; std::thread imu_worker_thread, pose_worker_thread;
@ -99,6 +100,7 @@ void image_save_worker() {
if (image_data_queue.try_pop(img)) { if (image_data_queue.try_pop(img)) {
if (recording) { if (recording) {
for (size_t cam_id = 0; cam_id < NUM_CAMS; ++cam_id) { 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 #if CV_MAJOR_VERSION >= 3
cam_data[cam_id] << img->t_ns << "," << img->t_ns << ".webp" cam_data[cam_id] << img->t_ns << "," << img->t_ns << ".webp"
<< std::endl; << std::endl;