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)