Created
October 12, 2017 17:12
-
-
Save anonymous/2dea6dd8c98b1aebdffd798f5af9827a to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python | |
import os | |
import sys | |
import gzip | |
import struct | |
import numpy as np | |
import zlib | |
import sparse_pose_graph_pb2 | |
import matplotlib.pyplot as plt | |
def pbstreamreader(posefile): | |
print "Reading .pbstream data" | |
idnumber = '0x7b1d1f7b5bf501db' | |
filehandle = open(posefile, 'rb') | |
# make sure the data | |
size = readthefile(filehandle) | |
if idnumber!= size: | |
print "unknown string" | |
return | |
#get the total size of the data | |
size = readsize(filehandle) | |
#print size | |
compressed_trajectory_data = readdatafromfile(filehandle,int(size)) | |
trajectory_data = zlib.decompress(compressed_trajectory_data,16+zlib.MAX_WBITS) | |
# read the entire data of the file | |
pose_graph=sparse_pose_graph_pb2.SparsePoseGraph() | |
pose_graph.ParseFromString(trajectory_data) | |
maptoddler_trajectory = [] | |
#print len(pose_graph.trajectory) | |
for i in xrange(0,len(pose_graph.trajectory)): | |
for j in xrange(0,len(pose_graph.trajectory[i].node)): | |
maptoddler_trajectory.append(pose_graph.trajectory[i].node[j]) | |
#plot_poses(maptoddler_trajectory) | |
filehandle.close() | |
return maptoddler_trajectory | |
def plot_poses(trajectory): | |
x = [] | |
y = [] | |
for i in xrange(0,len(trajectory)): | |
x.append(trajectory[i].pose.translation.x) | |
y.append(trajectory[i].pose.translation.y) | |
print len(x) | |
print len(y) | |
plt.plot(x,y,'r.') | |
plt.axis([min(x), max(x), min(y), max(y)]) | |
plt.show() | |
def readthefile(fileobj): | |
size =0 | |
byte=None | |
word = [] | |
for i in range(0,8): | |
byte = fileobj.read(1).encode("hex") | |
word.append(byte) | |
word = word[::-1] | |
word = '0x'+''.join(word) | |
return word | |
def readsize(fileobj): | |
return struct.unpack_from("<Q", fileobj.read(8))[0] | |
def readdatafromfile(fileobj, size): | |
return fileobj.read(size) | |
def main(): | |
if len(sys.argv)!=2: | |
print "Incorrect usage. rosrun maptoddlerpostprocessing generateassetsfrombag.py <path/to/the/*.pbstream>" | |
return | |
trajectory = pbstreamreader(sys.argv[1]) | |
if __name__=='__main__': | |
main() | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment