dwm1000: Added service call to calibrate center
authorErik Andresen <erik@vontaene.de>
Sat, 14 Jul 2018 17:30:41 +0000 (19:30 +0200)
committerErik Andresen <erik@vontaene.de>
Sat, 14 Jul 2018 17:30:41 +0000 (19:30 +0200)
CMakeLists.txt
scripts/dwm1000.py
srv/DWM1000Center.srv [new file with mode: 0644]

index 9dc330d..fd596fa 100644 (file)
@@ -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(
index a1dac53..bf58657 100755 (executable)
@@ -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
@@ -0,0 +1 @@
+---