import tf
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
+from wild_thumper.srv import DWM1000Center, DWM1000CenterResponse
if VISULAIZE:
import matplotlib.pyplot as plt
return x
+ def set_measure_cov(self, R):
+ if R > 0:
+ self.R = R
+
class DW1000(threading.Thread):
def __init__(self, name, addr, offset):
threading.Thread.__init__(self)
self.dist = 0
self.offset = offset
self.addr = addr
+ self.name = name
+ self.last_update = datetime.min
self.pub = rospy.Publisher(name, Float32, queue_size=16)
def distance(self):
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):
- while True:
- self.dist = self.get_value() + self.offset
- self.pub.publish(self.distance())
+ last_val = 10
+ while not rospy.is_shutdown():
+ val = self.get_value()
+ if abs(val - last_val) > 50:
+ rospy.logwarn("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):
- P_est = 0.003 # Fehlerkovarianz
- Q = 10e-4 # Systemrauschen
- R = 0.1 # Varianz des Messfehlers
- self.filter_x = simple_kalman(1.0, P_est, Q, R)
- self.filter_y = simple_kalman(0.0, P_est, Q, R)
+ # Varianz des Messfehlers
+ Rx = 0.2
+ Ry = 0.05
+ # Fehlerkovarianz
+ P_est_x = 0.02
+ P_est_y = 0.01
+ # Systemrauschen
+ Qx = 0.001
+ Qy = 0.00001
+ # TODO: Replace 2x 1d kalman with 2d kalman
+ # so the polyfit variance can be replaced with a covariance matrix
+ self.filter_x = simple_kalman(1.0, P_est_x, Qx, Rx)
+ self.filter_y = simple_kalman(0.0, P_est_y, Qy, Ry)
self.speed_x = 0
self.speed_y = 0
self.speed_z = 0
self.filter_x.x_est = pos[0]
self.filter_y.x_est = pos[1]
- # run kalman
- x = self.filter_x.run(x)
- y = self.filter_y.run(y)
+ # 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)
+
+ # Update variance
+ dist = np.linalg.norm([x, y])
+ self.filter_x.set_measure_cov(np.polyval([0.018, 0.0, 0.0], dist))
+ self.filter_y.set_measure_cov(np.polyval([0.006, 0.0, 0.0], dist))
+ else:
+ x = self.filter_x.x_est
+ y = self.filter_y.x_est
self.last_time = current_time
return x,y
+def handle_center_call(req):
+ diff = dwleft.distance_valid() - dwright.distance_valid()
+ dwleft.offset -= diff/2
+ dwright.offset += diff/2
+ rospy.loginfo("Centering to %.2f %.2f", dwleft.offset, dwright.offset)
+ return DWM1000CenterResponse()
+
if __name__ == "__main__":
- rospy.init_node('DW1000')
+ rospy.init_node('DWM1000', log_level=rospy.DEBUG)
dwleft = DW1000("uwb_dist_left", 0xc2, +0.0)
dwright = DW1000("uwb_dist_right", 0xc0, -0.0)
- dist_l_r = 0.285
+ dist_l_r = 0.285 # Distance between both DWM1000
rate = rospy.Rate(10)
pos = Position()
tf_broadcaster = tf.broadcaster.TransformBroadcaster()
+ rospy.Service('/DWM1000/center', DWM1000Center, handle_center_call)
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()
+ x = None
+ y = None
+ if dist_left == None or dist_right == None:
+ rospy.logerr_throttle(10, "no valid sensor update %r %r" % (dist_left, dist_right))
+ # run kalman prediction only
+ x,y = 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
+ rospy.logdebug("%.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)
+ rospy.logdebug("x=%.2f, y=%.2f", x, y)
+ # Rotate 90 deg
+ x, y = (y, -x)
+
+ x, y = pos.filter(x, y)
+
+ 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 %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()
+ # No current position, still need up update kalman prediction
+ x, y = pos.filter(None, None)
+ tf_broadcaster.sendTransform((x, y, 0.0), (0, 0, 0, 1), rospy.Time.now(), "uwb_beacon", "base_footprint")
rate.sleep()
projects / ros_wild_thumper.git / blobdiff