MASTER/py_dvl.py at sonar_mapping · Bluerov2/MASTER · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
#!/usr/bin/env python


##
##	This program reads the values from csv files generated by "save_data.py"
##  Convert the DVL datas into a trajectory estimation
##	Plot the estimated and real results
##


import matplotlib.pyplot as plt
import numpy as np
import os
import csv
import math


##############
##############
# PARAMETERS #
##############
##############

# Position of the DVL on the robot, where (0,0,0) is the center of gravity of the robot
OFFSET_X = 0.0
OFFSET_Y = 0.75
OFFSET_Z = 0.0


#########################################################################################################
##
## Lists where the datas from csv file will be saved
##

# -----  DVL  -----
dvl_timestamp = []
dvl_x = []
dvl_y = []
dvl_z = []
# -----  Robot  -----
robot_timestamp = []
robot_x = []
robot_y = []
robot_z = []
robot_yaw = []

##
## Read csv files and save them into the lists previously created, then remove the first value (which is '0')
##

# -----  DVL  -----
my_path = os.path.abspath(os.path.dirname(__file__))
path = os.path.join(my_path, "data/dvl.csv")
with open(path) as csv_file:
	csv_reader = csv.DictReader(csv_file, delimiter=',')
	for row in csv_reader:
		dvl_timestamp.append(float(row['time']))
		dvl_x.append(float(row['X']))
		dvl_y.append(float(row['Y']))
		dvl_z.append(float(row['Z']))
del dvl_timestamp[0]
del dvl_x[0]
del dvl_y[0]
del dvl_z[0]
# -----  Robot  -----
my_path = os.path.abspath(os.path.dirname(__file__))
path = os.path.join(my_path, "data/robot.csv")
with open(path) as csv_file:
	csv_reader = csv.DictReader(csv_file, delimiter=',')
	for row in csv_reader:
		robot_timestamp.append(float(row['time']))
		robot_x.append(float(row['X']))
		robot_y.append(float(row['Y']))
		robot_z.append(float(row['Z']))
		robot_yaw.append(float(row['Yaw']))
del robot_timestamp[0]
del robot_x[0]
del robot_y[0]
del robot_z[0]
del robot_yaw[0]
#########################################################################################################

##
## Convert the datas from dvl to an estimated trajectory
##

previous_time = 0
time = 0
dt = 0
estimated_traj_x = []
estimated_traj_y = []
estimated_traj_z = []
real_traj_x = []
real_traj_y = []
real_traj_z = []

# We do the assumption that the robot turns only around the z axis.
prevYaw = 0;

for i in range(len(robot_timestamp)-1):
	for j in range(len(dvl_timestamp)):
		# Find a match between the two times
		if robot_timestamp[i] <= dvl_timestamp[j] and robot_timestamp[i+1] > dvl_timestamp[j]:
			# Take the 1st values
			if len(estimated_traj_x) == 0:
				previous_time = dvl_timestamp[j]
				estimated_traj_x.append(robot_x[i])
				estimated_traj_y.append(robot_y[i])
				estimated_traj_z.append(robot_z[i])
				prevYaw = robot_yaw[i]
			else:
				time = dvl_timestamp[j]
				dt = float(time - previous_time)
				previous_time = time
				X = dvl_x[j] - OFFSET_X*(robot_yaw[i]-prevYaw)/dt
				Y = dvl_y[j] - OFFSET_Y*(robot_yaw[i]-prevYaw)/dt
				prevYaw = robot_yaw[i]
				estimated_traj_x.append(estimated_traj_x[-1] - X * dt * math.cos(robot_yaw[i]) + Y * dt * math.sin(robot_yaw[i]) )
				estimated_traj_y.append(estimated_traj_y[-1] - X * dt * math.sin(robot_yaw[i]) - Y * dt * math.cos(robot_yaw[i]) )
				estimated_traj_z.append(estimated_traj_z[-1] - dvl_z[j] * dt )

			# Reduce the number of values for the real trajectory
			real_traj_x.append(robot_x[i])
			real_traj_y.append(robot_y[i])
			real_traj_z.append(robot_z[i])
#########################################################################################################

##
##	Plot the results
##

"""
path = os.path.join(os.path.abspath(os.path.dirname(__file__)), "data/trajectories.csv")
with open(path, mode='w') as csv_file:
	writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
	writer.writerow(['estimation_X','estimation_Y','estimation_Z','real_X','real_Y','real_Z'])
	for i in range(len(estimated_traj_x)):
		writer.writerow([estimated_traj_x[i], estimated_traj_y[i], estimated_traj_z[i], real_traj_x[i], real_traj_y[i], real_traj_z[i]])
"""

plt.title("trajectories")
plt.plot(estimated_traj_x,estimated_traj_y,'r')
plt.plot(real_traj_x,real_traj_y,'b')
plt.show()

# Error:
error = []
for i in range(len(estimated_traj_x)):
	error.append(math.sqrt((estimated_traj_x[i]-real_traj_x[i])**2+(estimated_traj_y[i]-real_traj_y[i])**2))

plt.title("error")
plt.plot(error)
plt.show()