import struct
import rospy
import tf
-from std_msgs.msg import Float32
+import numpy as np
from math import *
+from datetime import datetime
from i2c import i2c
from time import sleep
+from std_msgs.msg import Float32
+from nav_msgs.msg import Odometry
if VISULAIZE:
import matplotlib.pyplot as plt
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.name = name
+ self.last_update = datetime.min
self.pub = rospy.Publisher(name, Float32, queue_size=16)
return ret[0]
def distance(self):
- return self.dist_filter
+ return self.dist
+
+ # Returns each distance only if current
+ def distance_valid(self):
+ if (datetime.now() - self.last_update).seconds < 1:
+ return self.dist
+ return None
def run(self):
+ last_val = 10
while True:
- self.dist = self.get_value() + self.offset
- self.dist_filter = self.filter.run(self.dist)
- self.pub.publish(self.dist_filter)
+ val = self.get_value()
+ if abs(val - last_val) > 10:
+ print "Ignoring values too far apart %s: %.2f - %.2f" % (self.name, val, last_val)
+ elif not isnan(val):
+ self.dist = val + self.offset
+ self.last_update = datetime.now()
+ self.pub.publish(self.distance())
+ last_val = val
sleep(0.1)
+class Position:
+ def __init__(self):
+ # Varianz des Messfehlers
+ Rx = 0.2
+ Ry = 0.05
+ # Fehlerkovarianz
+ P_est_x = 0.02
+ P_est_y = 0.01
+ # Systemrauschen
+ Q = 0.002
+ self.filter_x = simple_kalman(1.0, P_est_x, Q, Rx)
+ self.filter_y = simple_kalman(0.0, P_est_y, Q, Ry)
+ self.speed_x = 0
+ self.speed_y = 0
+ self.speed_z = 0
+ self.last_time = rospy.Time.now()
+ rospy.Subscriber("/odom_combined", Odometry, self.odomReceived)
+
+ def odomReceived(self, msg):
+ self.speed_x = msg.twist.twist.linear.x
+ self.speed_y = msg.twist.twist.linear.y
+ self.speed_z = msg.twist.twist.angular.z
+
+ """
+ TODO:
+ - variance of kalman should be dependant on distance
+ """
+ def filter(self, x, y):
+ # Correct estimation with speed
+ current_time = rospy.Time.now()
+ dt = (current_time - self.last_time).to_sec()
+ # Subtract vehicle speed
+ pos = np.array([self.filter_x.x_est, self.filter_y.x_est])
+ # translation
+ pos -= np.array([self.speed_x*dt, self.speed_y*dt])
+ # rotation
+ rot = np.array([[np.cos(self.speed_z*dt), -np.sin(self.speed_z*dt)],
+ [np.sin(self.speed_z*dt), np.cos(self.speed_z*dt)]])
+ pos = np.dot(pos, rot)
+ # copy back
+ self.filter_x.x_est = pos[0]
+ self.filter_y.x_est = pos[1]
+
+ # run kalman if new measurements are valid
+ if x != None and y != None:
+ x = self.filter_x.run(x)
+ y = self.filter_y.run(y)
+ else:
+ x = self.filter_x.x_est
+ y = self.filter_y.x_est
+
+ self.last_time = current_time
+ return x,y
+
+
if __name__ == "__main__":
rospy.init_node('DW1000')
- dwleft = DW1000("uwb_dist_left", 0xc2, +0.0)
- dwright = DW1000("uwb_dist_right", 0xc0, -0.0)
+ dwleft = DW1000("uwb_dist_left", 0xc2, +0.02)
+ dwright = DW1000("uwb_dist_right", 0xc0, -0.02)
dist_l_r = 0.285
rate = rospy.Rate(10)
+ pos = Position()
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
+ dist_left = dwleft.distance_valid()
+ dist_right = dwright.distance_valid()
+ if dist_left == None or dist_right == None:
+ print "no valid sensor update"
+ # run kalman prediction only
+ pos.filter(None, None)
+ else:
+ 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 %s" % (dwleft.distance(), dwright.distance(), 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
+ # Rotate 90 deg
+ x, y = (y, -x)
+
+ x, y = pos.filter(x, y)
+ tf_broadcaster.sendTransform((x, y, 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()
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()
+ # No current position, still need up update kalman prediction
+ pos.filter(None, None)
rate.sleep()