X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fdwm1000.py;h=bf58657c90a129dbe9579f7475f9b2db9af91a64;hp=a1dac533a75fdff3df52e15de79150042db8869f;hb=67dc60206f09f8e712d96b2b0c847e563f4c46ac;hpb=d0c13eed893ea8d50dd8f6471af5a19c625d7842

diff --git a/scripts/dwm1000.py b/scripts/dwm1000.py
index a1dac53..bf58657 100755
--- a/scripts/dwm1000.py
+++ b/scripts/dwm1000.py
@@ -14,6 +14,7 @@ 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
 
@@ -138,14 +139,22 @@ class Position:
 		return x,y
 
 
+def handle_center_call(req):
+	diff = dwleft.distance_valid() - dwright.distance_valid()
+	dwleft.offset -= diff/2
+	dwright.offset += diff/2
+	print "Centering to %.2f %.2f" % (dwleft.offset, dwright.offset)
+	return DWM1000CenterResponse()
+
 if __name__ == "__main__":
-	rospy.init_node('DW1000')
+	rospy.init_node('DWM1000')
 	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()
+	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()