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)
def run(self):
last_val = 10
- while True:
+ while not rospy.is_shutdown():
val = self.get_value()
- if abs(val - last_val) > 10:
- print "Ignoring values too far apart %s: %.2f - %.2f" % (self.name, val, last_val)
+ 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()
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)
+ 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.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()
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
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')
- dwleft = DW1000("uwb_dist_left", 0xc2, +0.02)
- dwright = DW1000("uwb_dist_right", 0xc0, -0.02)
- dist_l_r = 0.285
+ 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 # 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_valid()
dist_right = dwright.distance_valid()
+ x = None
+ y = None
if dist_left == None or dist_right == None:
- print "no valid sensor update"
+ rospy.logerr_throttle(10, "no valid sensor update %r %r" % (dist_left, dist_right))
# run kalman prediction only
- pos.filter(None, None)
+ x,y = pos.filter(None, None)
else:
dir = "left" if (dist_left < dist_right) else "right"
else:
dist_left += off/2
dist_right -= off/2
- print "%.2f %.2f %.2f %.2f %s" % (dwleft.distance(), dwright.distance(), dist_left, dist_right, dir)
+ 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)
- print x,y
+ rospy.logdebug("x=%.2f, y=%.2f", 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)
plt.show()
else:
# No current position, still need up update kalman prediction
- pos.filter(None, None)
+ 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()