Added dwm1000 script to localize tf beacon
[ros_wild_thumper.git] / 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()