def simulate(self):
D = 124
S = 60
tt = np.linspace(0, pi / 2, 6)
#ref = np.vstack((100 + 75*np.cos(tt), 200 + 75*np.sin(tt)))
ref = self.p
r = 0
fi0 = atan2(np.diff(ref[1, 0:2])[0], np.diff(ref[0, 0:2])[0])
fi0 = round(fi0 / pi * 2) * pi / 2
q0 = np.array(
[ref[0, 0] - D * cos(fi0), ref[1, 0] - D * sin(fi0), fi0, fi0])
qF0 = np.array([q0[0] + D * cos(q0[2]), q0[1] + D * sin(q0[2])])
qS0 = np.array([q0[0] + S * cos(q0[2]), q0[1] + S * sin(q0[2])])
q = q0
qF = qF0
qS = qS0
Q = np.array([np.hstack((q, qF, qS))])
opt = (0, 20**2, (0, 0))
while True:
ex = ref[0, r] - qF[0]
ey = ref[1, r] - qF[1]
ed = sqrt(ex**2 + ey**2)
ea = atan2(ey, ex) - q[3]
ea = wrapToPi(ea)
if ed < 2:
r = r + 1
if r >= ref.shape[1]:
break
Kd = 0.5
Ka = 2
v = Kd * ed
if v > 5:
v = 5
v = v * cos(ea)
w = v * sin(q[3] - q[2]) / D + Ka * ea
q[0] += v * cos(q[2]) * cos(q[3] - q[2])
q[1] += v * sin(q[2]) * cos(q[3] - q[2])
q[2] += v * sin(q[3] - q[2]) / D
q[3] += w
q[2] = wrapToPi(q[2])
q[3] = wrapToPi(q[3])
qF = np.array([q[0] + D * cos(q[2]), q[1] + D * sin(q[2])])
qS = np.array([q[0] + S * cos(q[2]), q[1] + S * sin(q[2])])
dQ = np.hstack((q, qF, qS))
Q = np.vstack((Q, dQ))
return Q
def _autoCalibrate(self):
if self._autoState == 0:
if not self.ongnd or self._autoTime is None:
self.setWheelVel(0., 0.)
self._autoTime = time()
else:
self.setVel(0.0, 2.0*pi/5.0*2.1)
if time() - self._autoTime > 5:
self._autoState = 1
self._autoTime = time()
elif self._autoState == 1:
ea = wrapToPi(0.0 - self.alpha)
omega = ea * 20.0
if omega > 2.0:
omega = 2.0
elif omega < -2.0:
omega = -2.0
self.setVel(0.0, omega)
if abs(ea) < 0.01:
self._autoState = 2
elif self._autoState == 2:
self.setWheelVel(0.0, 0.0)
if time() - self._autoTime > 0.5:
self._ready = True
def _handleOdometry(self, msg):
D = 0.1207
# x, y, phi = 0.0, 0.0, 0.0 # pravo x, y in phi pozicijo dobimo iz handleTag?
delta_d = msg.twist.twist.linear.x
gama = msg.twist.twist.angular.z
self.Q[0][0] = (delta_d * self.Q[0][0])**2
Qk = self.Q
Rk = self.R
phi = self.q[2]
A = np.array([[1, 0, -delta_d * sin(phi) * cos(gama)],
[0, 1, delta_d * cos(phi) * cos(gama)], [0, 0, 1]])
F = np.array([[cos(phi) * cos(gama), -delta_d * cos(phi) * sin(gama)],
[sin(phi) * cos(gama), -delta_d * sin(phi) * sin(gama)],
[1 / D * sin(gama), delta_d / D * cos(gama)]])
A_T = np.transpose(A)
C_T = np.transpose(self.C)
F_T = np.transpose(F)
#### Predikcija ####
# Odometrija
q_k1k1 = self.q
P_k1k1 = self.P
dq_kk1 = np.array([[delta_d * cos(gama) * cos(phi)],
[delta_d * cos(gama) * sin(phi)],
[delta_d * sin(gama) / D]])
q_kk1 = q_k1k1 + dq_kk1
P_kk1 = np.dot(np.dot(A, P_k1k1), A_T) + np.dot(np.dot(F, Qk), F_T)
if self.realTag != self.oldTag: # Če ni enak staremu tagu se izvede še korekcija
#### Korekcija ####
K1 = np.dot(P_kk1, C_T)
K2 = np.linalg.inv(
np.dot(self.C, np.dot(P_kk1, C_T)) +
Rk) # preveri če je množenje matrik v pravilnem vrstnem redu
K = np.dot(K1, K2)
# Zadnji prebran tag
zk = np.array([
tagPoses[self.realTag]
]) # Preveri ali vsebuje samo ID taga ali tudi pozicijo taga
zk[0][0] /= 1000
zk[0][1] /= 1000
zk_str = np.dot(self.C, q_kk1)
inovacija_zk = np.transpose(zk) - zk_str
inovacija_zk[2][0] = wrapToPi(inovacija_zk[2][0])
q_kk = q_kk1 + np.dot(K, inovacija_zk)
P_kk = P_kk1 - np.dot(K, np.dot(self.C, P_kk1))
self.q = q_kk
self.P = P_kk
self.oldTag = self.realTag
else:
self.q = q_kk1
self.P = P_kk1
print(self.q)
#'''
trans = TransformStamped()
trans.header.stamp = rospy.Time.now()
trans.header.frame_id = 'world'
trans.child_frame_id = 'jaka/estimate'
trans.transform.translation.x = self.q[0]
trans.transform.translation.y = self.q[1]
trans.transform.rotation.z = sin(self.q[2] / 2.0)
trans.transform.rotation.w = cos(self.q[2] / 2.0)
self._tfBroadcaster.sendTransform(trans)
'''
def _handleEncoders(self, left, right, heading):
#print('left = %d, right = %d, heading = %d' % (left, right, heading)) # Debug
self.alpha = wrapToPi(-float(heading)*2.0*pi/8192.0)
def compute(self):
# Simulations (move to static Edge method)
sim = {}
for edgeName, edge in self.edges.iteritems():
sim[edgeName] = edge.simulate()
self.detTags = Node.createNodes(world.TAGS)
# Find detected tags and edge breaks
edgeBreaks = {}
SIGMA_VIRTUAL = 5
SIGMA_REAL = 20
SIGMA_MAX = max(SIGMA_REAL, SIGMA_VIRTUAL)
for tagName, tag in self.tags.iteritems():
opt = (None, SIGMA_MAX**2, (0, 0))
for edgeName, edge in self.edges.iteritems():
for i in range(sim[edgeName].shape[0]):
qS = sim[edgeName][i, 6:8]
qF = sim[edgeName][i, 4:6]
if tag.isVirtual():
d = np.sum((qF - tag.p)**2)
tol = SIGMA_VIRTUAL**2
else:
d = np.sum((qS - tag.p)**2)
tol = SIGMA_REAL**2
if d < tol and d < opt[1]:
opt = (edge.name, d, (qF[0], qF[1]))
if opt[0] is not None:
self.detTags[tagName].p = opt[2]
edge = self.edges[opt[0]]
d = edge.pointToDistance(opt[2])
a = (tagName, d)
if opt[0] not in edgeBreaks:
edgeBreaks[opt[0]] = []
edgeBreaks[opt[0]].append(a)
# Sort edge breaks
for edgeBreak in edgeBreaks.itervalues():
edgeBreak.sort(key=lambda x: x[1])
# Find end parts
partsEnd = {}
for k, line in edgeBreaks.iteritems():
line.sort(key=lambda x: x[1])
partsEnd[k] = (line[-1][0], self.edges[k].length - line[-1][1])
# Find start parts
partsStart = {}
for k, line in edgeBreaks.iteritems():
line.sort(key=lambda x: x[1])
partsStart[k] = (line[0][0], line[0][1])
self.detEdges = Edge.createEdges()
tagEdges = {}
for k, line in edgeBreaks.iteritems():
# Find edges between the tags on the same line
if len(line) > 1:
for l in range(len(line) - 1):
a = line[l][0]
b = line[l + 1][0]
lab = line[l + 1][1] - line[l][1]
tagEdges[a] = [(b, lab)]
edge = Edge()
edge.name = '{}-{}'.format(a, b)
edge.setPoints(self.edges[k].getPart(
line[l][1], line[l + 1][1]))
self.detEdges[edge.name] = edge
end = self.edgeNodes[k][1]
start = []
for t, n in self.edgeNodes.iteritems():
if n[0] == end:
start.append(t)
a = partsEnd[k]
tagEdges[a[0]] = []
for s in start:
if s in partsStart:
b = partsStart[s]
tagEdges[a[0]].append((b[0], a[1] + b[1]))
pa = self.edges[k].getPart(self.edges[k].length - a[1])
pb = self.edges[s].getPart(0.0, b[1])
edge = Edge()
edge.name = '{}-{}'.format(a[0], b[0])
edge.setPoints(np.hstack((pa, pb[:, 1:])))
self.detEdges[edge.name] = edge
# Clean duplicated points
for n, e in self.detEdges.iteritems():
z = abs(np.sum((e.p[0:2, 1:] - e.p[0:2, 0:-1])**2, axis=0)) > 1e-6
if not np.all(z):
z = np.hstack((True, z))
e.p = e.p[:, z]
detEdgeNodes = Edge.connectNodes(self.detEdges, self.detTags)
detNodeEdges = Node.connectEdges(self.detTags, self.detEdges)
self.links = {}
for tagName in self.detTags:
self.links[tagName] = []
visited = []
for n, c in detNodeEdges.iteritems():
if len(c[0]) > 1:
psi = [
0,
] * len(c[0])
s = [
0,
] * len(c[0])
for i, e in enumerate(c[0]):
s[i] = self.detEdges[e].p.shape[1]
z = min(s)
a = self.detEdges[c[0][0]].p[0:2, 0:z]
m = [
0,
] * (len(c[0]) - 1)
for i in range(1, len(c[0])):
b = self.detEdges[c[0][i]].p[0:2, 0:z]
g = np.sum((b - a)**2, 0)
m[i - 1] = np.min(np.where(g > 1))
m = max(m)
for i, e in enumerate(c[0]):
d = np.diff(self.detEdges[c[0][i]].p[0:2,
[max([0, m - 1]), m]])
psi[i] = atan2(d[1], d[0])
for i in range(1, len(psi)):
psi[i] = wrapToPi(psi[i] - psi[0])
psi[0] = 0.0
o = sorted(range(len(psi)), key=lambda k: psi[k])
oo = [
None,
] * len(o)
for l, v in enumerate(reversed(o)):
oo[l] = c[0][v]
ab = detEdgeNodes[oo[0]]
self.links[ab[0]].append(('L', ab[1], oo[0]))
for l in range(1, len(oo) - 1):
ab = detEdgeNodes[oo[l]]
self.links[ab[0]].append(('X', ab[1], oo[l]))
ab = detEdgeNodes[oo[-1]]
self.links[ab[0]].append(('R', ab[1], oo[-1]))
for v in c[0]:
if v not in visited:
visited.append(v)
if len(c[1]) > 1:
psi = [
0,
] * len(c[1])
s = [
0,
] * len(c[1])
for i, e in enumerate(c[1]):
s[i] = self.detEdges[e].p.shape[1]
z = min(s)
a = np.fliplr(self.detEdges[c[1][0]].p)[0:2, 0:z]
m = [
0,
] * (len(c[1]) - 1)
for i in range(1, len(c[1])):
b = np.fliplr(self.detEdges[c[1][i]].p)[0:2, 0:z]
g = np.sum((b - a)**2, 0)
m[i - 1] = np.min(np.where(g > 1))
m = max(m)
for i, e in enumerate(c[1]):
d = np.diff(
np.fliplr(
self.detEdges[c[1][i]].p)[0:2,
[max([0, m - 1]), m]])
psi[i] = atan2(d[1], d[0])
for i in range(1, len(psi)):
psi[i] = wrapToPi(psi[i] - psi[0])
psi[0] = 0.0
o = sorted(range(len(psi)), key=lambda k: psi[k])
oo = [
None,
] * len(o)
for l, v in enumerate(o):
oo[l] = c[1][v]
ab = detEdgeNodes[oo[0]]
self.links[ab[0]].append(('L', ab[1], oo[0]))
for l in range(1, len(oo) - 1):
ab = detEdgeNodes[oo[l]]
self.links[ab[0]].append(('X', ab[1], oo[l]))
ab = detEdgeNodes[oo[-1]]
self.links[ab[0]].append(('R', ab[1], oo[-1]))
for v in c[1]:
if v not in visited:
visited.append(v)
rest = set(self.detEdges.keys()) - set(visited)
for r in rest:
ab = detEdgeNodes[r]
self.links[ab[0]].append(('X', ab[1], r))
self.computeTagPoses()
self.computeTagMap()