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
def __init__(self):
self.timestamp_list = []
self.curvature_list = []
self.last_angular_velocity_z = None
self.imu_angular_velocity = ImuAngularVelocity()
self.imu_speed = ImuSpeed()
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
class ImuAvCurvature:
def __init__(self):
self.timestamp_list = []
self.curvature_list = []
self.last_angular_velocity_z = None
self.imu_angular_velocity = ImuAngularVelocity()
self.imu_speed = ImuSpeed()
def add(self, location_est):
timestamp_sec = location_est.header.timestamp_sec
self.imu_angular_velocity.add(location_est)
self.imu_speed.add(location_est)
angular_velocity_z \
= self.imu_angular_velocity.get_latest_corrected_angular_velocity()
speed_mps = self.imu_speed.get_lastest_speed()
if speed_mps > 0.03:
kappa = angular_velocity_z / speed_mps
else:
kappa = 0
self.timestamp_list.append(timestamp_sec)
self.curvature_list.append(kappa)
self.last_angular_velocity_z = angular_velocity_z
def get_timestamp_list(self):
return self.timestamp_list
def get_curvature_list(self):
return self.curvature_list
def get_last_timestamp(self):
if len(self.timestamp_list) > 0:
return self.timestamp_list[-1]
return None
def get_last_curvature(self):
if len(self.curvature_list) > 0:
return self.curvature_list[-1]
return None
folders = sys.argv[1:]
fig, ax = plt.subplots()
colors = ["g", "b", "r", "m", "y"]
markers = ["o", "o", "o", "o"]
for i in range(len(folders)):
folder = folders[i]
color = colors[i % len(colors)]
marker = markers[i % len(markers)]
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()
colors = ["g", "b", "r", "m", "y"]
markers = [".", ".", ".", "."]
speed_jerk_dict = {}
for i in range(len(folders)):
x = []
y = []
folder = folders[i]
color = colors[i % len(colors)]
marker = markers[i % len(markers)]
fns = [f for f in listdir(folder) if isfile(join(folder, f))]
fns.sort()
for fn in fns:
reader = RecordItemReader(folder + "/" + fn)
jerk_processor = ImuSpeedJerk(True)
speed_processor = ImuSpeed(True)
topics = ["/apollo/localization/pose"]
for data in reader.read(topics):
if "pose" in data:
pose_data = data["pose"]
speed_processor.add(pose_data)
jerk_processor.add(pose_data)
data_x = grid(speed_processor.get_speed_list(), i + 1)
data_y = grid(jerk_processor.get_jerk_list(), i + 1)
data_x = data_x[-1 * len(data_y):]
x.extend(data_x)
y.extend(data_y)
speed_jerk_dict = generate_speed_jerk_dict(speed_jerk_dict, x, y)
def __init__(self):
self.timestamp_list = []
self.acc_list = []
self.imu_speed = ImuSpeed()
def __init__(self, is_lateral=False):
self.timestamp_list = []
self.acc_list = []
self.imu_speed = ImuSpeed(is_lateral)
