scripts/dwm1000.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3
   4 VISULAIZE = False
   5
   6 import threading
   7 import struct
   8 import rospy
   9 import tf
  10 from std_msgs.msg import Float32
  11 from math import *
  12 from i2c import i2c
  13 from time import sleep
  14 if VISULAIZE:
  15         import matplotlib.pyplot as plt
  16
  17 class simple_kalman:
  18         def __init__(self, x_est, P_est, Q, R):
  19                 self.x_est = x_est # Systemzustand
  20                 self.P_est = P_est # Fehlerkovarianz
  21                 self.Q = Q # Systemrauschen
  22                 self.R = R # Varianz des Messfehlers
  23
  24         def run(self, y):
  25                 # Korrektur mit der Messung
  26                 # (1) Berechnung der Kalman Verstärkung
  27                 K = self.P_est/(self.R + self.P_est)
  28                 # (2) Korrektur der Schätzung mit der Messung y
  29                 x = self.x_est + K*(y - self.x_est)
  30                 # (3) Korrektur der Fehlerkovarianzmatrix
  31                 P = (1-K)*self.P_est
  32                 #
  33                 # Prädiktion
  34                 # (1) Prädiziere den Systemzustand
  35                 self.x_est = x
  36                 # (2) Präzidiere die Fehlerkovarianzmatrix
  37                 self.P_est = P + self.Q
  38
  39                 return x
  40
  41 class DW1000(threading.Thread):
  42         def __init__(self, name, addr, offset):
  43                 threading.Thread.__init__(self)
  44                 self.setDaemon(1)
  45                 self.dist = 0
  46                 self.dist_filter = 0
  47                 self.offset = offset
  48                 self.addr = addr
  49
  50                 x_est = 1.0 # Systemzustand
  51                 P_est = 0.003 # Fehlerkovarianz
  52                 Q = 10e-4 # Systemrauschen
  53                 R = 0.05 # Varianz des Messfehlers
  54                 self.filter = simple_kalman(x_est, P_est, Q, R)
  55
  56                 self.pub = rospy.Publisher(name, Float32, queue_size=16)
  57
  58                 self.start()
  59
  60         def get_value(self):
  61                 dev = i2c(self.addr)
  62                 ret = struct.unpack("f", dev.read(4))
  63                 dev.close()
  64                 return ret[0]
  65
  66         def distance(self):
  67                 return self.dist_filter
  68
  69         def run(self):
  70                 while True:
  71                         self.dist = self.get_value() + self.offset
  72                         self.dist_filter = self.filter.run(self.dist)
  73                         self.pub.publish(self.dist_filter)
  74                         sleep(0.1)
  75
  76 if __name__ == "__main__":
  77         rospy.init_node('DW1000')
  78         dwleft  = DW1000("uwb_dist_left",  0xc2, +0.0)
  79         dwright = DW1000("uwb_dist_right", 0xc0, -0.0)
  80         dist_l_r = 0.285
  81         rate = rospy.Rate(10)
  82         tf_broadcaster = tf.broadcaster.TransformBroadcaster()
  83
  84         while not rospy.is_shutdown() and dwleft.is_alive() and dwright.is_alive():
  85                 dist_left = dwleft.distance()
  86                 dist_right = dwright.distance()
  87                 dir = "left" if (dist_left < dist_right) else "right"
  88
  89                 diff = abs(dist_left - dist_right)
  90                 if diff >= dist_l_r:
  91                         # difference to high, correct to maximum
  92                         off = diff - dist_l_r + 0.01
  93                         if dist_left > dist_right:
  94                                 dist_left -= off/2
  95                                 dist_right += off/2
  96                         else:
  97                                 dist_left += off/2
  98                                 dist_right -= off/2
  99                 print "%.2f %.2f %.2f %.2f %.2f %.2f %s" % (dwleft.dist, dwright.dist, dwleft.dist_filter, dwright.dist_filter, dist_left, dist_right, dir)
 100
 101                 a_r = (-dist_right**2 + dist_left**2 - dist_l_r**2) / (-2*dist_l_r)
 102                 x = dist_l_r/2 - a_r
 103                 t = dist_right**2 - a_r**2
 104                 if t >= 0:
 105                         y = sqrt(t)
 106                         print x,y
 107                         # Rotated 90 deg
 108                         tf_broadcaster.sendTransform((y, -x, 0.0), (0, 0, 0, 1), rospy.Time.now(), "uwb_beacon", "base_footprint")
 109
 110                         if VISULAIZE:
 111                                 circle_left = plt.Circle((-dist_l_r/2, 0), dwleft.distance, color='red', fill=False)
 112                                 circle_right = plt.Circle((dist_l_r/2, 0), dwright.distance, color='green', fill=False)
 113                                 plt.gca().add_patch(circle_left)
 114                                 plt.gca().add_patch(circle_right)
 115                                 plt.grid(True)
 116                                 plt.axis('scaled')
 117                                 plt.show()
 118
 119                 rate.sleep()