From 67dc60206f09f8e712d96b2b0c847e563f4c46ac Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Sat, 14 Jul 2018 19:30:41 +0200
Subject: [PATCH] dwm1000: Added service call to calibrate center

---
 CMakeLists.txt        |  9 ++++-----
 scripts/dwm1000.py    | 11 ++++++++++-
 srv/DWM1000Center.srv |  1 +
 3 files changed, 15 insertions(+), 6 deletions(-)
 create mode 100644 srv/DWM1000Center.srv

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9dc330d..fd596fa 100644
--- 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 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()
diff --git a/srv/DWM1000Center.srv b/srv/DWM1000Center.srv
new file mode 100644
index 0000000..ed97d53
--- /dev/null
+++ b/srv/DWM1000Center.srv
@@ -0,0 +1 @@
+---
-- 
2.39.5