class ImuSpeedAcc:
def __init__(self):
self.timestamp_list = []
self.acc_list = []
self.imu_speed = ImuSpeed()
def add(self, location_est):
self.imu_speed.add(location_est)
speed_timestamp_list = self.imu_speed.get_timestamp_list()
if len(speed_timestamp_list) > 5:
speed_list = self.imu_speed.get_speed_list()
acc = (speed_list[-1] - speed_list[-5]) / \
(speed_timestamp_list[-1] - speed_timestamp_list[-5])
self.acc_list.append(acc)
self.timestamp_list.append(speed_timestamp_list[-1])
def get_acc_list(self):
return self.acc_list
def get_timestamp_list(self):
return self.timestamp_list
def get_lastest_acc(self):
if len(self.acc_list) > 0:
return self.acc_list[-1]
else:
return None
def get_lastest_timestamp(self):
if len(self.timestamp_list) > 0:
return self.timestamp_list[-1]
else:
return None
class ImuSpeedAcc:
def __init__(self, is_lateral=False):
self.timestamp_list = []
self.acc_list = []
self.imu_speed = ImuSpeed(is_lateral)
def add(self, location_est):
self.imu_speed.add(location_est)
speed_timestamp_list = self.imu_speed.get_timestamp_list()
index_50ms = len(speed_timestamp_list) - 1
found_index_50ms = False
last_timestamp = speed_timestamp_list[-1]
while index_50ms >= 0:
current_timestamp = speed_timestamp_list[index_50ms]
if (last_timestamp - current_timestamp) >= 0.05:
found_index_50ms = True
break
index_50ms -= 1
if found_index_50ms:
speed_list = self.imu_speed.get_speed_list()
acc = (speed_list[-1] - speed_list[index_50ms]) / \
(speed_timestamp_list[-1] - speed_timestamp_list[index_50ms])
self.acc_list.append(acc)
self.timestamp_list.append(speed_timestamp_list[-1])
def get_acc_list(self):
return self.acc_list
def get_timestamp_list(self):
return self.timestamp_list
def get_lastest_acc(self):
if len(self.acc_list) > 0:
return self.acc_list[-1]
else:
return None
def get_lastest_timestamp(self):
if len(self.timestamp_list) > 0:
return self.timestamp_list[-1]
else:
return None
fns = [f for f in listdir(folder) if isfile(join(folder, f))]
fns.sort()
for fn in fns:
print(fn)
reader = RecordItemReader(folder + "/" + fn)
curvature_processor = ImuAvCurvature()
speed_processor = ImuSpeed()
av_processor = ImuAngularVelocity()
last_pose_data = None
last_chassis_data = None
for data in reader.read(["/apollo/localization/pose"]):
if "pose" in data:
last_pose_data = data["pose"]
curvature_processor.add(last_pose_data)
speed_processor.add(last_pose_data)
av_processor.add(last_pose_data)
data_x = curvature_processor.get_timestamp_list()
data_y = curvature_processor.get_curvature_list()
ax.scatter(data_x, data_y, c=color, marker=marker, alpha=0.4)
data_x = speed_processor.get_timestamp_list()
data_y = speed_processor.get_speed_list()
ax.scatter(data_x, data_y, c='r', marker=marker, alpha=0.4)
data_x = av_processor.get_timestamp_list()
data_y = av_processor.get_corrected_anglular_velocity_list()
ax.scatter(data_x, data_y, c='b', marker=marker, alpha=0.4)
plt.show()