注意一下几点
- 使用matplotlib在的pyplot包画图
- 使用open打开文件,使用readline()分行读取
- 使用numpy.delete(arrayname,index)删除确定位置处的数据
- 使用and和or而不是&&和||
- 一个figure上画多条线不用hold on, 且线色自动变化
代码如下:
import matplotlib.pyplot as plt
import numpy as ny
def draw_posexy(filepath):
pose_xy = []
pose_theta = []
pose = []
with open(filepath, 'r') as f:
while True:
line = f.readline()
if not line:
break
temp = line.strip('\n').split(',')
temp = list(map(lambda x: float(x), temp))
pose.append(temp)
pose_x = ny.array([i[1] for i in pose])
pose_y = ny.array([i[2] for i in pose])
pose_theta = [i[3] for i in pose]
index = []
for i in range(len(pose)):
if int(abs(pose_x[i])) > 1000 or int(abs(pose_y[i])) > 1000:
#print(i)
index.append(i)
elif i > 0 :
#if int(abs(pose_x[i] - pose_x[i - 1])) > 5 or int(abs(pose_y[i] - pose_y[i - 1])) > 5:
#print("jap:" + i.__str__())
#index.append(i)
if (int(pose_x[i]) < -80 and int(pose_x[i-2]) > -40) or (int(pose_x[i - 2]) < -80 and int(pose_x[i]) > -40):
print("middle ++++++++++++++" + i.__str__() + ": " + float(pose_x[i]).__str__() + "," + float(pose_y[i]).__str__())
#print(index)
for i in range(len(index)):
print(index[i].__str__() + ":")
print(float(pose_x[index[i]]).__str__() + "," + float(pose_y[index[i]]).__str__())
pose_x = ny.delete(pose_x, index)
pose_y = ny.delete(pose_y, index)
return pose_x, pose_y, pose_theta
filepath = "/home/efan/MyCode/data/slipTest_data/2020-01-19_15-40-40/text_pose_out_noimu.txt"
filepath_1 = "/home/efan/MyCode/data/slipTest_data/2020-01-19_15-40-40/text_pose_out_imu.txt"
[pos_x, pos_y, pos_theta] = draw_posexy(filepath)
[pos_x_1, pos_y_1, pos_theta_1] = draw_posexy(filepath_1)
#
plt.plot(pos_x_1, pos_y_1, '--')
plt.plot(pos_x, pos_y)
plt.show()