def velocity_to_pose(self):
# Read Rosbag and save encoder data
b1 = bagreader(self.bag1_filepath)
b2 = bagreader(self.bag2_filepath)
self.bag1_encoder_topic_data = pd.read_csv(b1.message_by_topic(topic = '/encoder_pub'))
self.bag2_encoder_topic_data = pd.read_csv(b2.message_by_topic(topic = '/encoder_pub'))
left_encoder_list_bag1 = self.bag1_encoder_topic_data['left'].to_list()
right_encoder_list_bag1 = self.bag1_encoder_topic_data['right'].to_list()
left_encoder_list_bag2 = self.bag2_encoder_topic_data['left'].to_list()
right_encoder_list_bag2 = self.bag2_encoder_topic_data['right'].to_list()
pose_x_list = []
pose_y_list = []
angle_list = []
count_list = []
# Convert angular velocity for angle caliberation
for i in range(len(left_encoder_list_bag2)):
v_x, v_y, w = self.encoder_to_velocity(right_encoder_list_bag2[i], left_encoder_list_bag2[i])
dt = 0.1
dtheta = w*dt
self.pose.theta = (self.pose.theta + dtheta)
angle_list.append(self.pose.theta*180/math.pi)
count_list.append(i)
self.pose.theta = 0.0
# Convert Encoder data to Pose data for linear caliberation
for i in range(len(left_encoder_list_bag1)):
v_x, v_y, w = self.encoder_to_velocity(right_encoder_list_bag1[i], left_encoder_list_bag1[i])
dt = 0.1
dx = (v_x*math.cos(self.pose.theta) - v_y*math.sin(self.pose.theta))*dt
dy = (v_x*math.sin(self.pose.theta) + v_y*math.cos(self.pose.theta))*dt
dtheta = w*dt
self.pose.x = self.pose.x + dx
self.pose.y = self.pose.y + dy
self.pose.theta = (self.pose.theta + dtheta)
pose_x_list.append(self.pose.x)
pose_y_list.append(self.pose.y)
# Plot the Pose values for linear caliberation
fig, (ax1, ax2) = plt.subplots(1, 2)
fig.suptitle('Caliberation plots')
#ax1.xlabel('Pose X values')
#ax1.xlabel('Pose y values')
ax1.plot(pose_x_list, pose_y_list, 'b-')
ax2.plot(count_list, angle_list, 'r-')
plt.show()
def bag2csv(bag_path: Path, topics: List[str]) -> None:
bag = bagreader(str(bag_path.resolve()))
# convert topics to csv files
# csv files will be stored in a folder with the name of the bag
for topic in topics:
bag.message_by_topic(topic)
def __init__(self, planner, file_name, bag_name):
odom_topic = "/sensorsim/police/odom"
collision_topic = "/sensorsim/police/collision"
self.bag = bagreader(bag_name)
eps = self.split_runs(odom_topic, collision_topic)
self.evalPath(planner, file_name, eps)
self.nc_total = 0
def bag_to_dataframe(bagpath: Path, topics: List[str]) -> Dict[str, DataFrame]:
bag = bagpy.bagreader(bagpath)
dataframes = dict()
for topic in topics:
topic_msgs = bag.message_by_topic(topic)
dataframes[topic] = pd.read_csv(topic_msgs)
return dataframes
def eda_func(indir, outdir, bag):
'''
Part of the data pipeline to transform our raw '.bag' file into csv data for further EDA (exploratory data analysis)
note: was able to utilize bagpy and rosbag on the ucsdets/scipy-ml-notebook docker image
'''
# first create the data directory
directory = "data"
parent_dir = "./"
path = os.path.join(parent_dir, directory)
#remove data dir if one already exists
if (os.path.exists(path) and os.path.isdir(path)):
shutil.rmtree(path)
os.mkdir(path)
os.mkdir(outdir)
print('data directory successfully created')
bag_path = os.path.join(indir, bag)
b = bagreader(bag_path)
# velmsgs = b.vel_data()
# print(velmsgs)
# veldf = pd.read_csv(velmsgs[0])
# Read Laser Data
laser_data = b.laser_data()
laser_df = pd.read_csv(laser_data[0])
laser_df.to_csv(os.path.join(outdir, 'test_laser_data.csv'))
print('Successfully added laser data from .bag file to path')
# Read Velocity Data
velocity_data = b.vel_data()
velocity_df = pd.read_csv(velocity_data[0])
velocity_df.to_csv(os.path.join(outdir, 'test_velocity_data.csv'))
print('Successfully added velocity data from .bag file to path')
# Read Standard Messages
standard_data = b.std_data()
standard_df = pd.read_csv(standard_data[0])
standard_df.to_csv(os.path.join(outdir, 'test_standard_data.csv'))
print('Successfully added standard data from .bag file to path')
# Read Odometry Data
odometry_data = b.odometry_data()
odometry_df = pd.read_csv(odometry_data[0])
odometry_df.to_csv(os.path.join(outdir, 'test_odometry_data.csv'))
print('Successfully added odometry data from .bag file to path')
# Read Wrench Data
wrench_data = b.wrench_data()
wrench_df = pd.read_csv(wrench_data[0])
wrench_df.to_csv(os.path.join(outdir, 'test_wrench_data.csv'))
print('Successfully added wrench data from .bag file to path')
return
def showDialog(self, MainWindow):
options = QFileDialog.Options()
options |= QFileDialog.DontUseNativeDialog
self.fileName, _ = QFileDialog.getOpenFileName(None, "QFileDialog.getOpenFileName()", "",
"Rosbag Files (*.bag)", options=options)
if self.fileName:
self.statusbar.showMessage("File loaded: " + self.fileName)
self.bag = bagreader(self.fileName)
topics_list = self.bag.topic_table["Topics"].values.tolist()
for topic in topics_list:
self.comboBox_topic_1.addItem(topic)
self.comboBox_topic_2.addItem(topic)
def bag_to_dataframes(bagpath: Path,
topics: List[str]) -> Dict[str, DataFrame]:
bag = bagpy.bagreader(str(bagpath))
dataframes = dict()
for topic in topics:
topic_msgs: str = bag.message_by_topic(topic)
dataframes[topic] = pd.read_csv(topic_msgs)
columns_except_time = dataframes[topic].columns.difference(
[DFKeys.TIME.value])
return dataframes
def bagOpener(bag_path, topic_name):
output_path = bag_path.replace('raw', 'clean').replace('bag', 'csv')
split_path_arr = output_path.split("/")
topic_path = split_path_arr[0] + "/" + split_path_arr[1] + "/" + split_path_arr[2]
if not os.path.exists(topic_path):
print(topic_path)
os.mkdir(topic_path)
b = bagreader(bag_path)
data = b.message_by_topic(topic_name)
df = pd.read_csv(data)
df.to_csv(output_path)
shutil.rmtree(bag_path.replace(".bag", ""))
return
import rospy
import message_filters
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
import time
import rosbag
import bagpy
from bagpy import bagreader
import pandas as pd
b = bagreader(
'/home/lattepanda/catkin_ws/src/visual_odometry/bags/odom_bag.bag')
odom_data_file = b.odometry_data()
imu_data_file = b.message_by_topic('/imu_k64')
b.plot_odometry(save_fig=True)
#%%
import rosbag
bag = rosbag.Bag(bagFile)
for topic, msg, t in bag.read_messages(
topics=['/xsens/imu/acceleration', '/xsens/imu/angular_velocity']):
print(topic)
print(msg)
bag.close()
#%%
import bagpy
import pandas as pd
from bagpy import bagreader
b = bagreader("/home/thomas/Private/JetsonCar/logs/2020-12-15-18-41-39.bag")
print(*b.topics, sep='\n')
speed_file = b.message_by_topic('/xsens/imu/angular_velocity')
speed = pd.read_csv(speed_file)
fig, ax = bagpy.create_fig(1)
ax[0].scatter(x='Time',
y='vector.z',
data=speed,
s=3,
marker='o',
label='Original Speed')
ax[0].legend()
ax[0].set_xlabel('Time')
ax[0].set_ylabel('m/s')
#b = bagreader('bagfiles/Exp3_3_(30noise_30_10gripper)_2021-05-06-20-54-23.bag')
#exp = 33
#b = bagreader('bagfiles/Exp4_1_(0noise_45_0gripper)_2021-05-06-20-59-02.bag')
#exp = 41
#b = bagreader('bagfiles/Exp4_2_(0noise_45_5gripper)_2021-05-06-21-05-03.bag')
#exp = 42
#b = bagreader('bagfiles/Exp4_3_(0noise_45_10gripper)_2021-05-06-21-08-20.bag')
#exp = 43
#b = bagreader('bagfiles/Exp4_4_(0noise_45_15gripper)_2021-05-06-21-13-37.bag')
#exp = 44
batch = '12'
b = bagreader('Deep_Learning_Data/deep_learning_data_' + batch + '.bag')
# Get the list of topics available in the file
print(b.topic_table)
# Read each topic
# a. Wrench topic: forces and torques
data_f = b.message_by_topic('wrench')
forces = pd.read_csv(data_f)
# b. Joint_States topic: angular positions of the 6 joints
data_j = b.message_by_topic('joint_states')
positions = pd.read_csv(data_j)
data_l = b.message_by_topic('deep_learning_label')
labels_msg = pd.read_csv(data_l)
def __init__(self):
self.TotalVehs = 7 # including LV
self.BagNo = 2 # LV manuever number
self.Direction = 1 # +1== North, (-1)==South
### ****PARAMETERS ****
# self.Gamma1=1.0 # Packet reception rate (out of 1.0)
self.CurrentVel = 0 # Start condition
self.timeHeadway = 0.57 # In seconds
self.L = 1 #in meters
# self.Kp=0.2
# self.Kv=0.3
# self.Ka=0.6
# self.BrakeDecelGain=2 # Unused
# #Gilbert Model Parameters:
# self.BadLR=95
# self.GoodToBad=30
# self.BadToGood=30
# self.CurrentGilbertState=1 # 1= Good State; 0 = Bad State
# self.NextGilbertState=1
# self.PLFactor=1
## **** GPS ORIGIN ****
self.lat0 = 30.632913
self.lon0 = -96.481894 # deg
self.h0 = 54.3 # meters
## **** LOAD BAG FILES ****
self.Pos = []
self.Vel = []
self.Accel = []
for vIdx in range(self.TotalVehs):
if vIdx == 0:
CarBag = bagreader('/home/mkz/mkzbag/2021Oct8/OkTune9/LVBag' +
str(self.BagNo) + '.bag')
# CarBag=bagreader('/home/vamsi/Downloads/ATemp/2021Oct6/LVBag1.bag')
# CarBag=bagreader('V:\OneDrive - Texas A&M University\Research\CACC Research\Lincoln\BagFiles\2021Oct6\LVBag1.bag')
else:
prevVehPath = '/home/mkz/mkzbag/2021Oct8/OkTune9/Bag' + str(
self.BagNo) + 'FV' + str(vIdx) + '_CACC1.bag'
# prevVehPath='/home/vamsi/Downloads/ATemp/2021Oct6/FV'+str(vIdx)+'_CACC1.bag'
# prevVehPath='V:\OneDrive - Texas A&M University\Research\CACC Research\Lincoln\BagFiles\2021Oct6\FV'+str(vIdx)+'_CACC1.bag'
CarBag = bagreader(prevVehPath)
Bagfile = CarBag.message_by_topic(
topic='/piksi/navsatfix_best_fix')
TempPos = np.loadtxt(open(Bagfile, "rb"),
delimiter=",",
skiprows=1,
usecols=(0, 7, 8,
9)) # [Time (s). Lat, Lon, Alt]
Bagfile = CarBag.message_by_topic(
topic='/vehicle/brake_info_report')
TempAccel = np.loadtxt(
open(Bagfile, "rb"), delimiter=",", skiprows=1,
usecols=(0, 8)) # [Time (s). Accel_over_ground]
Bagfile = CarBag.message_by_topic(topic='/vehicle/steering_report')
TempSpeed = np.loadtxt(open(Bagfile, "rb"),
delimiter=",",
skiprows=1,
usecols=(0, 9)) # [Time (s), speed]
startTime = TempPos[0, 0]
for idx in range(TempPos.shape[0]):
TempPos[idx, 0] = TempPos[idx, 0] - startTime
tempVal = pm.geodetic2enu(TempPos[idx, 1], TempPos[idx, 2],
TempPos[idx, 3], self.lat0,
self.lon0, self.h0) # Convert to ENU
TempPos[idx, 1] = tempVal[0]
TempPos[idx, 2] = tempVal[1] # North, (X), of interest.
TempPos[idx, 3] = tempVal[2]
startTime = TempSpeed[0, 0]
for idx in range(TempSpeed.shape[0]):
TempSpeed[idx, 0] = TempSpeed[idx, 0] - startTime
startTime = TempAccel[0, 0]
for idx in range(TempAccel.shape[0]):
TempAccel[idx, 0] = TempAccel[idx, 0] - startTime
self.Pos.append(TempPos)
self.Vel.append(TempSpeed)
self.Accel.append(TempAccel)
self.plotter()
import bagpy
from bagpy import bagreader
c = bagreader(
"/home/isr/rosbag/tiltedVLP_xsens_odom_zed/negative_obstacle.bag")
c.laser_data()
import bagpy
from bagpy import bagreader
b = bagreader('file path')
b.topic_table
velmsgs = b.vel_data()
velmsgs
import pandas as pd
velf = pd.read_csv(velmsgs[0])
velf
newmsg = b.message_by_topic(topic='topic name')
newmsg
def __init__(self):
## **** CONFIGURATION *****
self.PL_Enable = True # Packet losses are enabled, if true
self.VehNumber = 6 # FV number
self.BagNo = 2 # LV maneuver number
self.Direction = 1 # +1== North, (-1)==South
### ****PARAMETERS ****
self.Gamma1 = 1.0 # Packet reception rate (out of 1.0)
self.CurrentVel = 0 # Start condition
self.timeHeadway = 0.57 # In secaonds
self.L = 1 #in meters
self.Kp = 0.2
self.Kv = 0.3
self.Ka = 0.6
self.BrakeDecelGain = 2 # Unused
#Gilbert Model Parameters:
self.BadLR = 95
self.GoodToBad = 30
self.BadToGood = 30
self.CurrentGilbertState = 1 # 1= Good State; 0 = Bad State
self.NextGilbertState = 1
self.PLFactor = 1
## **** GPS ORIGIN ****
self.lat0 = 30.632913
self.lon0 = -96.481894 # deg
self.h0 = 54.3 # meters
## **** BAG FILES ****
if self.VehNumber == 1:
self.bLV = bagreader('/home/mkz/mkzbag/2021Oct8/LVBag' +
str(self.BagNo) + '.bag')
else:
prevVehPath = '/home/mkz/mkzbag/2021Oct8/Bag' + str(
self.BagNo) + 'FV' + str(self.VehNumber - 1) + '_CACC1.bag'
self.bLV = bagreader(prevVehPath)
#Position:
bLVfile1 = self.bLV.message_by_topic(topic='/piksi/navsatfix_best_fix')
bLVRaw = np.loadtxt(open(bLVfile1, "rb"),
delimiter=",",
skiprows=1,
usecols=(0, 7, 8, 9)) # [Time (s). Lat, Lon, Alt]
self.bLVPosData = np.zeros((bLVRaw.shape))
startTimebLV = bLVRaw[0, 0]
for idx in range(bLVRaw.shape[0]):
self.bLVPosData[idx, 0] = bLVRaw[idx, 0] - startTimebLV
tempVal = pm.geodetic2enu(bLVRaw[idx, 1], bLVRaw[idx, 2],
bLVRaw[idx,
3], self.lat0, self.lon0, self.h0)
self.bLVPosData[idx, 1] = tempVal[0]
self.bLVPosData[idx, 2] = tempVal[1]
self.bLVPosData[idx, 3] = tempVal[2]
#Accel:
bLVfile1 = self.bLV.message_by_topic(
topic='/vehicle/brake_info_report')
bLVRaw = np.loadtxt(open(bLVfile1, "rb"),
delimiter=",",
skiprows=1,
usecols=(0, 8)) # [Time (s). Accel_over_ground]
self.bLVAccelData = np.zeros((bLVRaw.shape))
startTimebLV = bLVRaw[0, 0]
for idx in range(bLVRaw.shape[0]):
self.bLVAccelData[idx, 0] = bLVRaw[idx, 0] - startTimebLV
self.bLVAccelData[idx, 1] = bLVRaw[idx, 1]
#Velocity:
bLVfile1 = self.bLV.message_by_topic(topic='/vehicle/steering_report')
bLVRaw = np.loadtxt(open(bLVfile1, "rb"),
delimiter=",",
skiprows=1,
usecols=(0, 9)) # [Time (s), speed]
self.bLVvelData = np.zeros((bLVRaw.shape))
startTimebLV = bLVRaw[0, 0]
for idx in range(bLVRaw.shape[0]):
self.bLVvelData[idx, 0] = bLVRaw[idx, 0] - startTimebLV
self.bLVvelData[idx, 1] = bLVRaw[idx, 1]
## *********** Other Init
self.acc_class = std_msgs.msg.Float32()
self.acc_pub = rospy.Publisher('x_acc/control_input',
std_msgs.msg.Float32,
queue_size=2)
rospy.Subscriber('time_topic', Time, self.timeStore)
rospy.Subscriber('vehicle/steering_report', SteeringReport,
self.velFun)
rospy.Subscriber('vehicle/brake_info_report',
dbw_mkz_msgs.msg.BrakeInfoReport, self.accelfun)
rospy.Subscriber('piksi/navsatfix_best_fix', NavSatFix, self.posfun)
rosbags = list(filter(rx.search, os.listdir('.')))
print("Choose a rosbag:")
i = 0
for rosbag1 in rosbags:
print(("[%d]: %s") % (i, rosbag1))
i += 1
inp = input()
if inp.isnumeric():
print("Opening: ", rosbags[int(inp)])
myrosbag = rosbags[int(inp)]
elif inp == "a" or inp == "A":
print("All")
else:
exit()
b = bagpy.bagreader(myrosbag)
## get the list of topics
#print(b.topic_table)
#
df_pose = pd.read_csv(b.message_by_topic('/current_pose'))
plt.scatter(x='pose.position.x', y='pose.position.y', data=df_pose, s=1)
"""
df_zed = pd.read_csv(b.message_by_topic('/zed_node/left/image_rect_color/compressed'))
"""
plt.show()
#!/usr/bin/env python
# coding: utf-8
# In[1]:
from bagpy import bagreader
import bagpy
import pandas as pd
import seaborn as sea
import matplotlib.pyplot as plt
b = bagreader('test3_09-23-59.bag')
# Read Laser Data
csv = b.laser_data()
df = pd.read_csv(csv[0])
# Read Velocity Data
ms = b.vel_data()
vel = pd.read_csv(ms[0])
# Read Standard Messages
s = b.std_data()
data = pd.read_csv(s[0])
# Read odometry Data
odom = b.odometry_data()
odomdata = pd.read_csv(odom[0])
# Read Wrench Data
w = b.wrench_data()
# https://rahulbhadani.medium.com/reading-ros-messages-from-a-bagfile-in-python-b006538bb520
import bagpy
from bagpy import bagreader
import pandas as pd
import seaborn as sea
import matplotlib.pyplot as plt
import numpy as np
# read the .bag file given
b = bagreader('lidar_data/test2.bag')
# check the available topics
print(b.topic_table)
csvfiles = []
for t in b.topics:
data = b.message_by_topic(t)
csvfiles.append(data)
# to decode specific type
# data = b.message_by_topic('/scan')
data = b.message_by_topic('/rosout')
print("File saved: {}".format(data))
df_imu = pd.read_csv(data)
print(df_imu)
def multi_plot(
baglist,
message_type,
topic_name,
signal_name,
savepath="/home/ivory/CyverseData/ProjectSparkle/Consolidated Plot"):
'''
Create a multi-plot of a topic and signal on the same plot
For example, 6 bag files are sent as a `baglist`.One must specify type of messages
among std, vel, wrench, odom. One desires to plot `linear.x` from topic `cmd_vel`, then,
`topic_name = "cmd_vel"` and `signal_name="linear.x"` is passed to the function.
Parameters
-------------
baglist: `list`
A list of strings containing a list of full path of bagfiles
message_type: `str`
Message types among "std", "vel", "odometry", "laser", "wrench"
topic_name: `str`
signal_name: `str`
savepath: `str`
'''
fig, ax = bagpy.create_fig()
for bag in baglist:
br = bagreader(bag)
try:
files = eval("br." + message_type + "_data()")
except AttributeError:
print("Unsupported type {}. Aborting.".format(message_type))
return
search_criteria = topic_name.replace("/", "-")
required_files = []
for file in files:
if search_criteria in file:
required_files.append(file)
fig, ax = bagpy.create_fig(num_of_subplots=1)
for i, f in enumerate(required_files):
df = pd.read_csv(f)
if signal_name in df.columns:
ax.scatter(x='Time',
y=signal_name,
data=df,
marker='*',
linewidth=0.3,
s=9,
color=color_long[i])
else:
print(
"Specified component/signal {} unavailable in {}. Aborting! ".
format(signal_name, f))
return
ax.legend(required_files)
ax.set_title("Timseries plot for {}/{}".format(topic_name, signal_name),
fontsize=16)
ax.set_xlabel("Time", fontsize=14)
ax.set_ylabel("Time", fontsize=14)
current_fig = plt.gcf()
dt_object = datetime.datetime.fromtimestamp(time.time())
dt = dt_object.strftime('%Y-%m-%d-%H-%M-%S-%f')
fileToSave = savepath + "/" + dt + "_" + search_criteria + "_" + signal_name.replace(
".", "_")
with open(fileToSave + ".pickle", 'wb') as f:
pickle.dump(fig, f)
current_fig.savefig(fileToSave + ".pdf", dpi=100)
current_fig.savefig(fileToSave + ".png", dpi=100)
plt.show()
#!/usr/bin/env python
# coding: utf-8
# In[1]:
from bagpy import bagreader
import bagpy
import pandas as pd
import seaborn as sea
import matplotlib.pyplot as plt
b = bagreader(
'/home/ivory/CyverseData/JmscslgroupData/ARED/2016-07-28/data_by_test/Bag Files/test3_09-23-59.bag'
)
# Read Laser Data
csv = b.laser_data()
df = pd.read_csv(csv[0])
# Read Velocity Data
ms = b.vel_data()
vel = pd.read_csv(ms[0])
# Read Standard Messages
s = b.std_data()
data = pd.read_csv(s[0])
# Read odometry Data
odom = b.odometry_data()
odomdata = pd.read_csv(odom[0])
# Read Wrench Data
def __init__(self, bag_name):
# self.bag = rosbag.Bag(bag_name)
self.bag = bagreader(bag_name)
self.rbag = rosbag.Bag(bag_name)
from bagpy import bagreader
import pandas as pd
import seaborn as sea
import matplotlib.pyplot as plt
import numpy as np
bag_folder_path = join(getcwd(), 'autoferry_bags')
bag_names = listdir(bag_folder_path)
bag_paths = [join(bag_folder_path, bag_name) for bag_name in bag_names if bag_name.endswith('.bag')]
test_bag_name = '2020-09-15-14-29-27.bag'
test_bag_path = join(bag_folder_path, test_bag_name)
test_bag = bagreader(test_bag_path)
csv_dir = join(bag_folder_path, splitext(test_bag_name)[0])
# h x w = 2048 x 2448 for all cameras
front_image_topic = '/EO/F/image_raw'
front_left_image_topic = 'EO/FL/image_raw'
front_right_image_topic = 'EO/FR/image_raw'
image_topics = [front_image_topic, front_left_image_topic, front_right_image_topic]
# If already written csv files, do not do again..
assert len(listdir(csv_dir)) == 0, 'The csv directory is not empty and will be overwritten - sure you want this '
for t in tqdm(image_topics):
if t in test_bag.topics:
data = test_bag.message_by_topic(t)
else:
print('Skipped topic of name {} not found in test_bag'.format(t))
import bagpy
import matplotlib
from bagpy import bagreader
import os
import pandas as pd
from scipy.spatial.transform import Rotation as R
import string
import matplotlib.image as mpimg
import matplotlib.pyplot as plt
from matplotlib import patches
import pandas as pd
import numpy as np
matplotlib.use('Agg')
name = "0009"
b = bagreader('/mnt/new/bagfile/' + name + '.bag')
# replace the topic name as per your need
model_states, t_model = b.message_by_topic('/gazebo/model_states')
camera_infos, t_camera = b.message_by_topic('/rrbot/camera1/camera_info')
# df_laser = pd.read_csv(LASER_MSG)
# prints laser data in the form of pandas dataframe
# def world_camera(wor)
image_folder_name = '/mnt/new/guidedog/raw_image/' + name
entries = os.listdir('{}'.format(image_folder_name))
image_time_list = []
a = list(map(lambda x: x.replace('.png', ''), entries))
flag = False
for entry in a:
image_time_list.append(int(entry))
idx_max = len(image_time_list)
#!/usr/bin/env python # coding: utf-8 # In[10]: import bagpy from bagpy import bagreader import pandas as pd import numpy as np import matplotlib.pyplot as plt # In[2]: folder = '/home/ivory/CyverseData/ProjectSparkle/' filename = 'sparkle_n_2_update_rate_20.0_max_update_rate_100.0_time_step_0.01_logtime_150.0_2020-06-18-16-39-19.bag' b = bagreader(folder + filename) # In[3]: b.topic_table # In[4]: b.plot_odometry() # In[5]: b.plot_vel() # In[6]:
def __init__(self, bagfile):
self.bagfile = bagfile
self.bagreader = bagpy.bagreader(bagfile)
# coding: utf-8
import bagpy
from bagpy import bagreader
import matplotlib.pyplot as plt
import pandas as pd
import sys
# In[3]:
n = len(sys.argv)
print("Total arguments passed:", n)
if n != 2:
sys.exit("usage: script.py filepath")
b = bagreader(sys.argv[1])
# In[4]:
# b.topic_table
# In[5]:
CMD_ACC_MSG = b.message_by_topic('/timed_accel')
ACC_MSG = b.message_by_topic('/vehicle/accel')
GHOST_VEL_MSG = b.message_by_topic('/ghost/vehicle/vel')
VEL_MSG = b.message_by_topic('/vehicle/vel')
# In[8]:
df_cacc = pd.read_csv(CMD_ACC_MSG)
def animate_circle(circumference, n_vehicles, leader_vel, publish_rate, max_update_rate, time_step, log_time, include_laser, logdir, description="", plot=False, **kwargs):
'''
Sparkle Simulation API: `animate_circle`
Higher level API for performing simulation on a circular trajectory
Parameters
--------------
n_vehicles: `integer`
The number vehicles to be spawned in the simulation
publish_rate: `double`
The desired publish rate of the velocity of the vehicles' updated odometry to update Gazebo simulation
max_update_rate: `double`
Max Update Rate to be set in Gazebo Physics Simulation
time_step: `double`
Time step for Gazebo Physics Simulation
include_laser: `bool`
Boolean flag to include/exclude laser model and laser plugin on the first vehicle model
logdir: `string`
Directory/Path where bag files and other data files recording simulation statistics will be saved.
description: `string`
Any human readable description of this simulation, the default is empty string, useful for annotating bag file name, will be saved in the yaml file.
Returns
----------
`string`
A list of data files is returned. We can specify only certain information to retrieved from saved bag files at the end of the simulation.
'''
print("Simulation begin")
simConfig = {"circumference": circumference, "n_vehicles": n_vehicles, "update_rate": publish_rate, \
"log_time": log_time, "max_update_rate": max_update_rate, "time_step": time_step, \
"include_laser": include_laser, "logdir": logdir, "description": description}
print("Simulation Configuration: {}".format(simConfig))
C = circle(**simConfig)
# Start a simulation
bag = C.simulate(leader_vel, logdir=logdir)
print("Bagfile recorded is {}".format(bag))
# C.analyze()
# gz_stat_file = C.gzstatsfile
# print("GZStat file is {}".format(gz_stat_file))
# GZ = gzstats(gz_stat_file)
# GZ.plotRTF()
# GZ.plotSimStatus()
gz_stat_file = C.gzstatsfile
print("GZStat file is {}".format(gz_stat_file))
GZ = gzstats(gz_stat_file)
GZ.dataframe.to_csv(gz_stat_file[0:-4]+".csv")
csvfiles = []
if plot:
if bag is not None:
br = bagreader(bag)
csvfiles = br.odometry_data()
plot_topic_hz(csvfiles)
configfile = C.bagfile[0:-4] + "/" + "simConfig.yaml"
with open(configfile, 'w') as file:
documents = yaml.dump(simConfig, file)
time.sleep(6)
pt.close('all')
print("Simulation Ends.")
return bag
def read_data_into_csv(csvfiles, file_path):
b = bagreader(file_path)
for topic in b.topics:
raw_data = b.message_by_topic(topic)
csvfiles.append(raw_data)
