dwm1000: Added service call to calibrate center

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9dc330d47f5ea44eb4b1f570dcce68ed660e1c57..fd596fa7f4a0bb939a568ccd89d9576e5de168c2 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -48,11 +48,10 @@ add_message_files(
 )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  DWM1000Center.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(

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()

diff --git a/srv/DWM1000Center.srv b/srv/DWM1000Center.srv
new file mode 100644 (file)
index 0000000..ed97d53
--- /dev/null
+++ b/srv/DWM1000Center.srv
@@ -0,0 +1 @@
+---