-
Notifications
You must be signed in to change notification settings - Fork 11
/
Copy pathlocalization_uwb_withyaw.py
212 lines (185 loc) · 7.36 KB
/
localization_uwb_withyaw.py
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
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
#!/usr/bin/env python
# _*_ coding: utf-8 _*_
import sys
import math
import time
import numpy as np
import signal
from cyber_py import cyber
from modules.control.proto.chassis_pb2 import Chassis
from modules.localization.proto.localization_pb2 import localization
from modules.localization.proto.localization_pb2 import pos
from modules.sensors.proto.nooploop_pb2 import TagFrame
from modules.sensors.proto.sensors_pb2 import Gyro
class Exercise(object):
def __init__(self, node):
self.node = node
self.alpha = 0.01
self.maxTimes = 100
self.x = np.array([0.725, 0.7]);
# base station matrix
self.base_x = np.array([0, 0, 2.5, 2.5])
self.base_y = np.array([0, 1.32, 0, 1.32])
self.xi = np.array([])
self.yi = np.array([])
# distance
self.di = np.array([])
#self.di = np.array([8, 9.303, 0, 4.75])
self.localization = localization()
self.pos = pos()
self.vel_head = 0
self.yaw = 0
self.direction = 0
self.node.create_reader("/geek/gyro", Gyro, self.gyrocallback)
self.node.create_reader("/geek/uwb/pose", TagFrame, self.tagcallback)
self.node.create_reader("/chassis", Chassis, self.chassiscallback)
self.writer = self.node.create_writer("/geek/uwb/localization", pos)
self.fuseflag = 0
self.v_x = 0
self.head_x = 1
self.head_y = 0
self.v_y = 0
self.direction_lag = 0
signal.signal(signal.SIGINT, self.sigint_handler)
signal.signal(signal.SIGHUP, self.sigint_handler)
signal.signal(signal.SIGTERM, self.sigint_handler)
self.is_sigint_up = False
while True:
time.sleep(0.05)
if self.is_sigint_up:
print("Exit")
self.is_sigint_up = False
sys.exit()
def sigint_handler(self, signum, frame):
#global is_sigint_up
self.is_sigint_up = True
print("catched interrupt signal!")
def gyrocallback(self, gyro):
if (0):
#self.yaw -= 0.02 * gyro.gyro.z
if (self.fuseflag == 1):
self.yaw = self.yaw + 0.1 * (self.vel_head - self.yaw)
#self.yaw = self.yaw * 0.8 + 0.2 * self.vel_head
else:
gyro.gyro.z = -gyro.gyro.z * 0.02
head_x_new = (self.head_x * math.cos(gyro.gyro.z) - self.head_y * math.sin(gyro.gyro.z))
head_y_new = (self.head_x * math.sin(gyro.gyro.z) + self.head_y * math.cos(gyro.gyro.z))
self.head_x = head_x_new
self.head_y = head_y_new
if (self.fuseflag == 1):
lenth_v = math.sqrt(self.v_x * self.v_x + self.v_y * self.v_y)
self.head_x += 0.3 * (self.v_x / lenth_v - self.head_x);
self.head_y += 0.3 * (self.v_y / lenth_v - self.head_y);
self.yaw = math.atan2(self.head_y, self.head_x)
#self.yaw = self.vel_head
if (self.yaw > 3.14159):
self.yaw = self.yaw - 2 * 3.14159
if (self.yaw < -3.14159):
self.yaw = self.yaw + 2 * 3.14159
def chassiscallback(self, chassis):
if (chassis.speed > 0):
self.direction = 1
else:
self.direction = -1
if (abs(chassis.speed) < 0.1):
self.direction = 0
self.direction_lag = self.direction_lag * 0.5 + 0.5 * self.direction
print self.direction
def tagcallback(self, tag):
self.di = np.array([])
self.xi = np.array([])
self.yi = np.array([])
for k in range (0, 4):
if tag.dis[k].distance == 1:
continue
self.di = np.append(self.di, tag.dis[k].distance)
self.xi = np.append(self.xi, self.base_x[k])
self.yi = np.append(self.yi, self.base_y[k])
datas = np.array([[self.xi], [self.yi], [self.di]])
datas = datas.T
self.calcu_loss_fun(self.x, self.maxTimes, self.alpha, datas)
v_x = tag.vel.x
v_y = tag.vel.y
#if (v_x * v_x + v_y * v_y) > 0.2:
vel_head = math.atan2(v_y, v_x)
self.fuseflag = 0
if (self.direction_lag >= 0.95):
if (v_x * v_x + v_y * v_y) > 0.01:
self.fuseflag = 1
self.v_x = -v_x
self.v_y = -v_y
self.vel_head = vel_head + 3.14159
if (self.vel_head > 3.14159):
self.vel_head = self.vel_head - 2 * 3.14159
if (self.vel_head < -3.14159):
self.vel_head = self.vel_head + 2 * 3.14159
if 0 :
if (v_x * v_x + v_y * v_y) > 0.04:
self.fuseflag = 1
self.v_y = v_y
self.vel_head = vel_head + 3.14159
if (self.vel_head > 3.14159):
self.vel_head = self.vel_head - 2 * 3.14159
if (self.vel_head < -3.14159):
self.vel_head = self.vel_head + 2 * 3.14159
if self.direction == 0:
self.vel_head = self.vel_head
self.fuseflag = 0
#print self.vel_head
# 定义函数f(x)
def problem(self, x, data):
#e = 2.71828182845904590
return (x[0] - data[0,0]) ** 2 + (x[1] - data[0,1])**2 - data[0,2]**2
#定义损失函数
def loss_fun(self, x, datas):
sum_err = 0;
for data in datas:
sum_err += 0.5 * (self.problem(x, data) - 0)**2
return sum_err
#计算损失函数的斜率
def slope_fx(self, x, datas):
d = 0.01;
delta1 = np.array([d, 0]);
delta2 = np.array([0, d]);
J1 = (self.loss_fun(x+delta1, datas) - self.loss_fun(x-delta1, datas)) / (2.0*d)
J2 = (self.loss_fun(x+delta2, datas) - self.loss_fun(x-delta2, datas)) / (2.0*d)
return [J1, J2]
#代入f(x)，计算数值
def calcu_loss_fun(self, x, maxTimes, alpha, datas):
for i in range(maxTimes):
ret = self.slope_fx(x, datas)
x1 = x[0] - ret[0]*alpha;
x2 = x[1] - ret[1]*alpha;
x = np.array([x1, x2])
if self.loss_fun(x, datas) > 1000:
return
#print 'times %d, x: %.13f,y: %.13f f(x): %.13f' % (i, x[0],x[1], self.loss_fun(x, datas))
self.localization.apriltag0.x = x[0]
self.localization.apriltag0.y = -1
self.localization.apriltag0.z = x[1]
self.localization.apriltag0.yaw = -1
self.localization.apriltag1.x = x[0]
self.localization.apriltag1.y = -1
self.localization.apriltag1.z = x[1]
self.localization.apriltag1.yaw = -1
self.localization.apriltag.x = x[0]
self.localization.apriltag.y = -1
self.localization.apriltag.z = x[1]
self.localization.apriltag.yaw = -1
self.localization.predict.x = x[0]
self.localization.predict.y = -1
self.localization.predict.z = x[1]
self.localization.predict.yaw = -1
self.pos.x = x[0]
self.pos.y = x[1]
#self.pos.x = self.head_x
#self.pos.y = self.head_y
self.pos.z = self.vel_head
self.pos.yaw = self.yaw
self.writer.write(self.pos)
if __name__ == '__main__':
cyber.init()
exercise_node = cyber.Node("localization_node")
exercise = Exercise(exercise_node)
exercise_node.spin()
cyber.shutdown()