读取bag包中的imu信息，保存为TXT文件

# coding:utf-8
#!/usr/bin/python
 
# Extract images from a bag file.
 
#PKG = 'beginner_tutorials'
import roslib;   #roslib.load_manifest(PKG)
import os
import rosbag
import rospy
import cv2
import pylab
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.pyplot import MultipleLocator
from itertools import islice
import sys



bag_file = "123.bag"

def readDataFormBag():
	bag = rosbag.Bag(bag_file, "r")
	X = []
	Y = []	
	#info = bag.get_type_and_topic_info()
	bag_data = bag.read_messages('/imu/data')
	fy = open("imu_y.txt",'w')
	fz = open("imu_z.txt",'w')
	fx = open("imu_x.txt",'w')
	for topic, msg, t in bag_data:
		# acc_y = "%.6f" % msg.linear_acceleration.y
		# acc_x = "%.6f" % msg.linear_acceleration.x
		# acc_z = "%.6f" % msg.linear_acceleration.z
		acc_y = "%.6f" % msg.angular_velocity.y
		acc_x = "%.6f" % msg.angular_velocity.x
		acc_z = "%.6f" % msg.angular_velocity.z
		timestr = "%.6f" %  msg.header.stamp.to_sec()
		fx.write(timestr)
		fx.write(',')
		fx.write(acc_x)
		fx.write('\n')

		fy.write(timestr)
		fy.write(',')
		fy.write(acc_y)
		fy.write('\n')

		fz.write(timestr)
		fz.write(',')
		fz.write(acc_z)
		fz.write('\n')


	fx.close()
	fz.close()
	fy.close()

def plotData():
	 	X = []
		Y = []
		fr = open("imu_x111.txt",'r')
		for line in islice(fr,20980,20990):
		# for line in fr:
			 	trainingSet = line.split(',')
			 	X.append(trainingSet[0])
				print(trainingSet[1])
			 	Y.append(trainingSet[1])
		plt.plot(X,Y,color="blue")
		#y_major_locator=MultipleLocator(100)
		#ax=plt.gca()
		#ax.yaxis.set_major_locator(y_major_locator)
		plt.show()
def plotData2():
		X = []
		Y = []
		#IMU = ['9','10','11']
		names = ['1595904213','1595904214','1595904215','1595904216','1595904217','1595904218','1595904219','1595904220']
		fr = open("imu_x111.txt",'r')
		for line in islice(fr,250,300):
				trainingSet = line.split(',')
			 	X.append(trainingSet[0])
				print(trainingSet[1])
			 	Y.append(trainingSet[1])		
                plt.plot(X,Y,marker='o',mec='r',mfc='w')
		plt.legend(['true','Cubic-Spline'])
		plt.xticks(X, names, rotation=45)
		#plt.yticks(Y, IMU)
		#plt.margins(0)
		#plt.subplots_adjust(bottom=0.15)S
		plt.xlabel(u"time(s)")
		plt.ylabel("IMU")
		plt.title("A simple plot")
		plt.show()

#plotData2()
readDataFormBag()