dwm1000: Added service call to calibrate center

[ros_wild_thumper.git] / scripts / dwm1000.py

diff --git a/scripts/dwm1000.py b/scripts/dwm1000.py
index a1dac533a75fdff3df52e15de79150042db8869f..bf58657c90a129dbe9579f7475f9b2db9af91a64 100755 (executable)
--- 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()