Added dwm1000 script to localize tf beacon

[ros_wild_thumper.git] / scripts / dwm1000.py

diff --git a/scripts/dwm1000.py b/scripts/dwm1000.py
new file mode 100755 (executable)
index 0000000..06c6cbc
--- /dev/null
+++ b/scripts/dwm1000.py
@@ -0,0 +1,119 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+VISULAIZE = False
+
+import threading
+import struct
+import rospy
+import tf
+from std_msgs.msg import Float32
+from math import *
+from i2c import i2c
+from time import sleep
+if VISULAIZE:
+       import matplotlib.pyplot as plt
+
+class simple_kalman:
+       def __init__(self, x_est, P_est, Q, R):
+               self.x_est = x_est # Systemzustand
+               self.P_est = P_est # Fehlerkovarianz
+               self.Q = Q # Systemrauschen
+               self.R = R # Varianz des Messfehlers
+
+       def run(self, y):
+               # Korrektur mit der Messung
+               # (1) Berechnung der Kalman Verstärkung
+               K = self.P_est/(self.R + self.P_est)
+               # (2) Korrektur der Schätzung mit der Messung y
+               x = self.x_est + K*(y - self.x_est)
+               # (3) Korrektur der Fehlerkovarianzmatrix
+               P = (1-K)*self.P_est
+               #
+               # Prädiktion
+               # (1) Prädiziere den Systemzustand
+               self.x_est = x
+               # (2) Präzidiere die Fehlerkovarianzmatrix
+               self.P_est = P + self.Q
+
+               return x
+
+class DW1000(threading.Thread):
+       def __init__(self, name, addr, offset):
+               threading.Thread.__init__(self)
+               self.setDaemon(1)
+               self.dist = 0
+               self.dist_filter = 0
+               self.offset = offset
+               self.addr = addr
+
+               x_est = 1.0 # Systemzustand
+               P_est = 0.003 # Fehlerkovarianz
+               Q = 10e-4 # Systemrauschen
+               R = 0.05 # Varianz des Messfehlers
+               self.filter = simple_kalman(x_est, P_est, Q, R)
+
+               self.pub = rospy.Publisher(name, Float32, queue_size=16)
+
+               self.start()
+
+       def get_value(self):
+               dev = i2c(self.addr)
+               ret = struct.unpack("f", dev.read(4))
+               dev.close()
+               return ret[0]
+
+       def distance(self):
+               return self.dist_filter
+
+       def run(self):
+               while True:
+                       self.dist = self.get_value() + self.offset
+                       self.dist_filter = self.filter.run(self.dist)
+                       self.pub.publish(self.dist_filter)
+                       sleep(0.1)
+
+if __name__ == "__main__":
+       rospy.init_node('DW1000')
+       dwleft  = DW1000("uwb_dist_left",  0xc2, +0.0)
+       dwright = DW1000("uwb_dist_right", 0xc0, -0.0)
+       dist_l_r = 0.285
+       rate = rospy.Rate(10)
+       tf_broadcaster = tf.broadcaster.TransformBroadcaster()
+
+       while not rospy.is_shutdown() and dwleft.is_alive() and dwright.is_alive():
+               dist_left = dwleft.distance()
+               dist_right = dwright.distance()
+               dir = "left" if (dist_left < dist_right) else "right"
+
+               diff = abs(dist_left - dist_right)
+               if diff >= dist_l_r:
+                       # difference to high, correct to maximum
+                       off = diff - dist_l_r + 0.01
+                       if dist_left > dist_right:
+                               dist_left -= off/2
+                               dist_right += off/2
+                       else:
+                               dist_left += off/2
+                               dist_right -= off/2
+               print "%.2f %.2f %.2f %.2f %.2f %.2f %s" % (dwleft.dist, dwright.dist, dwleft.dist_filter, dwright.dist_filter, dist_left, dist_right, dir)
+
+               a_r = (-dist_right**2 + dist_left**2 - dist_l_r**2) / (-2*dist_l_r)
+               x = dist_l_r/2 - a_r
+               t = dist_right**2 - a_r**2
+               if t >= 0:
+                       y = sqrt(t)
+                       print x,y
+                       # Rotated 90 deg
+                       tf_broadcaster.sendTransform((y, -x, 0.0), (0, 0, 0, 1), rospy.Time.now(), "uwb_beacon", "base_footprint")
+
+                       if VISULAIZE:
+                               circle_left = plt.Circle((-dist_l_r/2, 0), dwleft.distance, color='red', fill=False)
+                               circle_right = plt.Circle((dist_l_r/2, 0), dwright.distance, color='green', fill=False)
+                               plt.gca().add_patch(circle_left)
+                               plt.gca().add_patch(circle_right)
+                               plt.grid(True)
+                               plt.axis('scaled')
+                               plt.show()
+
+               rate.sleep()