Commit 96b90739 authored by Nicolas Federico Furquez Morena's avatar Nicolas Federico Furquez Morena
Browse files

script para leer bags

parent f17b0e8e
#!/usr/bin/env python
'''
This script saves each topic in a bagfile as a csv.
Accepts a filename as an optional argument. Operates on all bagfiles in current directory if no argument provided
Written by Nick Speal in May 2013 at McGill University's Aerospace Mechatronics Laboratory
www.speal.ca
Supervised by Professor Inna Sharf, Professor Meyer Nahon
'''
import rosbag, sys, csv
import time
import string
import os #for file management make directory
import shutil #for file management, copy file
#verify correct input arguments: 1 or 2
if (len(sys.argv) > 2):
print("invalid number of arguments: " + str(len(sys.argv)))
print("should be 2: 'bag2csv.py' and 'bagName'")
print("or just 1 : 'bag2csv.py'")
sys.exit(1)
elif (len(sys.argv) == 2):
listOfBagFiles = [sys.argv[1]]
numberOfFiles = "1"
print("reading only 1 bagfile: " + str(listOfBagFiles[0]))
elif (len(sys.argv) == 1):
listOfBagFiles = [f for f in os.listdir(".") if f[-4:] == ".bag"] #get list of only bag files in current dir.
numberOfFiles = str(len(listOfBagFiles))
print("reading all " + numberOfFiles + " bagfiles in current directory: \n")
for f in listOfBagFiles:
print(f)
print("\n press ctrl+c in the next 10 seconds to cancel \n")
time.sleep(10)
else:
print("bad argument(s): " + str(sys.argv)) #shouldnt really come up
sys.exit(1)
count = 0
for bagFile in listOfBagFiles:
count += 1
print("reading file " + str(count) + " of " + numberOfFiles + ": " + bagFile)
#access bag
bag = rosbag.Bag(bagFile)
bagContents = bag.read_messages()
bagName = bag.filename
#create a new directory
folder = bagName.rstrip('.bag')
try: #else already exists
os.makedirs(folder)
except:
pass
shutil.copyfile(bagName, folder + '/' + bagName)
#get list of topics from the bag
listOfTopics = []
for topic, msg, t in bagContents:
if topic not in listOfTopics:
listOfTopics.append(topic)
for topicName in listOfTopics:
#Create a new CSV file for each topic
filename = folder + '/' + topicName.replace('/', '_slash_') + '.csv'
with open(filename, 'w+') as csvfile:
filewriter = csv.writer(csvfile, delimiter = ',')
firstIteration = True #allows header row
for subtopic, msg, t in bag.read_messages(topicName): # for each instant in time that has data for topicName
#parse data from this instant, which is of the form of multiple lines of "Name: value\n"
# - put it in the form of a list of 2-element lists
msgString = str(msg)
msgList = msgString.split('\n')
instantaneousListOfData = []
for nameValuePair in msgList:
splitPair = nameValuePair.split(':')
for i in range(len(splitPair)): #should be 0 to 1
splitPair[i] = splitPair[i].strip()
instantaneousListOfData.append(splitPair)
#write the first row from the first element of each pair
if firstIteration: # header
headers = ["rosbagTimestamp"] #first column header
for pair in instantaneousListOfData:
headers.append(pair[0])
filewriter.writerow(headers)
firstIteration = False
# write the value from each pair to the file
values = [str(t)] #first column will have rosbag timestamp
for pair in instantaneousListOfData:
if len(pair) > 1:
values.append(pair[1])
filewriter.writerow(values)
bag.close()
print("Done reading all " + numberOfFiles + " bag files.")
\ No newline at end of file
#!/usr/bin/env python
import sys
import os
import csv
import rosbag
import rospy
# ##################
# # DESCRIPTION:
# # Creates CSV files of the robot joint states from a rosbag (for visualization with e.g. pybullet)
# #
# # USAGE EXAMPLE:
# # rosrun your_package get_jstate_csvs.py /root/catkin_ws/bagfiles your_bagfile.bag
# # ##################
filename = sys.argv[2]
directory = sys.argv[1]
print("Reading the rosbag file")
if not directory.endswith("/"):
directory += "/"
extension = ""
if not filename.endswith(".bag"):
extension = ".bag"
bag = rosbag.Bag(directory + filename + extension)
# Create directory with name filename (without extension)
results_dir = directory + filename[:-4] + "_results"
if not os.path.exists(results_dir):
os.makedirs(results_dir)
print("Writing robot joint state data to CSV")
with open(results_dir +"/"+filename+'_joint_states.csv', mode='w') as data_file:
data_writer = csv.writer(data_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
data_writer.writerow(['time', 'msg'])
# Get all message on the /joint states topic
for topic, msg, t in bag.read_messages(topics=['/localization']):
# Only write to CSV if the message is for our robot
# if msg.name[0] == "robot_elbow_joint":
data_writer.writerow([t.to_sec(),msg])
print("Finished creating csv file!")
bag.close()
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment