def bag_to_power(filename='power.bag'):
bagfile = Bag(filename)
energies = {}
power = []
for topic, msg, timestamp in bagfile.read_messages():
if topic.find('energy') != -1:
energies[topic] = msg.data
power.append([timestamp.to_sec(), sum(energies.values())])
bagfile.close()
return numpy.array(power)
def bag_to_csv(filename):
bagfile = Bag(filename)
topics = init_topics(filename)
start_time = bagfile.get_start_time()
for topic, msg, timestamp in bagfile.read_messages(topics=topics.keys()):
formatter = topics[topic]['formatter']
data_line = formatter((topic,msg,timestamp,start_time))
topics[topic]['file'].write(', '.join(map(str,data_line)) + '\n')
for topic in topics.keys():
topics[topic]['file'].close()
bagfile.close()
def bag_to_blobs(filename):
bagfile = Bag(filename)
blobs = []
b_time = None
curr_blobs = [None, None]
poses = []
start_time = bagfile.get_start_time()
for topic, msg, timestamp in bagfile.read_messages():
if topic[-9:] == 'blob_list':
if len(msg.blobs) == 1:
m_time = msg.header.stamp.to_sec()
r_idx = int(topic[-20])
blob = [msg.blobs[0].x, msg.blobs[0].y]
#blobs.append([m_time,r_idx] + blob)
if b_time is None:
b_time = m_time
curr_blobs[r_idx] = blob
elif abs(b_time - m_time) < 0.001:
curr_blobs[r_idx] = blob
blobs.append([b_time] + curr_blobs[0] + curr_blobs[1])
b_time = None
curr_blobs = [None, None]
else:
b_time = m_time
curr_blobs = [None, None]
curr_blobs[r_idx] = blob
elif topic == '/tf':
for tf_msg in msg.transforms:
p_time = tf_msg.header.stamp.to_sec()
base_frame = tf_msg.header.frame_id
child_frame = tf_msg.child_frame_id
pos = tf_msg.transform.translation
pos = [pos.x, pos.y, pos.z]
ori = tf_msg.transform.rotation
ori = [ori.x, ori.y, ori.z, ori.w]
poses.append([p_time, base_frame, child_frame, pos, ori])
bagfile.close()
return blobs, poses
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import PointCloud2
from genpy.rostime import Time
from rosbag.bag import Bag
def callback_rgb(data, time, bag):
print 'received PointCloud2!'
data.header.stamp = time + rospy.Duration.from_sec(1505388514) # shift time stamp to the year 2017 (for kalibr)
bag.write('/ensenso/depth/points', data, t=data.header.stamp)
def rgb_listener(bag):
counter = 0
while(True):
raw_input("press enter to continue...")
counter += 1
time = rospy.Time(counter)
msg_rgb = rospy.wait_for_message('/ensenso/depth/points', PointCloud2)
callback_rgb(msg_rgb, time,bag)
if __name__ == '__main__':
rospy.init_node('rgb_listener', anonymous=True)
try:
bag = Bag('images_for_calibration.bag', 'w', allow_unindexed=True)
rgb_listener(bag)
finally:
bag.close()