mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
38 lines
1.1 KiB
Python
38 lines
1.1 KiB
Python
|
import rosbag
|
||
|
|
import tf
|
||
|
|
import sys
|
||
|
|
from tf.msg import tfMessage
|
||
|
|
|
||
|
|
if len(sys.argv) < 2:
|
||
|
|
print 'Usage: $ python extract_stereo.py "2012-01-25-12-14-25"'
|
||
|
|
sys.exit(0)
|
||
|
|
|
||
|
|
bagName = sys.argv[1]
|
||
|
|
with rosbag.Bag(bagName + '_stereo.bag', 'w') as outbag:
|
||
|
|
print 'Processing ' + bagName + '.bag...'
|
||
|
|
leftCamInfoStatus = True
|
||
|
|
leftImageStatus = True
|
||
|
|
rightCamInfoStatus = True
|
||
|
|
rightImageStatus = True
|
||
|
|
for topic, msg, t in rosbag.Bag(bagName + '.bag').read_messages():
|
||
|
|
if topic == "/tf":
|
||
|
|
outbag.write(topic, msg, t)
|
||
|
|
elif topic == "/wide_stereo/left/camera_info":
|
||
|
|
if leftCamInfoStatus:
|
||
|
|
outbag.write(topic, msg, t)
|
||
|
|
leftCamInfoStatus = not leftCamInfoStatus
|
||
|
|
elif topic == "/wide_stereo/right/camera_info":
|
||
|
|
if rightCamInfoStatus:
|
||
|
|
outbag.write(topic, msg, t)
|
||
|
|
rightCamInfoStatus = not rightCamInfoStatus
|
||
|
|
elif topic == "/wide_stereo/left/image_raw":
|
||
|
|
if leftImageStatus:
|
||
|
|
outbag.write(topic, msg, t)
|
||
|
|
leftImageStatus = not leftImageStatus
|
||
|
|
elif topic == "/wide_stereo/right/image_raw":
|
||
|
|
if rightImageStatus:
|
||
|
|
outbag.write(topic, msg, t)
|
||
|
|
rightImageStatus = not rightImageStatus
|
||
|
|
|
||
|
|
print 'Output: ' + bagName + '_out.bag'
|