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()