rosbagtocsv

最新推荐文章于 2024-06-22 16:54:37 发布

书生落拓

最新推荐文章于 2024-06-22 16:54:37 发布

阅读量345

点赞数

文章标签：算法 slam

本文链接：https://blog.csdn.net/weixin_39373577/article/details/121647998

版权

这段代码从名为'Normal_robot.bag'的ROS Bag文件中读取'/camera/imu'话题的数据，然后将线性加速度和角速度的y, x, z分量写入到imu.csv文件中。每行数据包含时间戳、角速度和加速度的各个分量。

摘要由CSDN通过智能技术生成

# 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 = "Normal_robot.bag"

def readDataFormBag():
	bag = rosbag.Bag(bag_file, "r")
	bag_data = bag.read_messages('/camera/imu')
	fx = open("imu.csv",'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
		w_y = "%.6f" % msg.angular_velocity.y
		w_x = "%.6f" % msg.angular_velocity.x
		w_z = "%.6f" % msg.angular_velocity.z
		#timestr = "%.9f" % msg.header.stamp.to_sec()
                timestr = msg.header.stamp.to_sec()
                timer = 1e9 * timestr
                imutime = "%d" % timer
		fx.write(imutime)
		fx.write(',')
                fx.write(w_x)
                fx.write(',')
                fx.write(w_y)
                fx.write(',')
                fx.write(w_z)
                fx.write(',')
		fx.write(acc_x)
                fx.write(',')
                fx.write(acc_y)
                fx.write(',')
                fx.write(acc_z)
		fx.write('\n')


	fx.close()
	
readDataFormBag()