dwm1000: Ignore outdated distance values

[ros_wild_thumper.git] / scripts / follow_uwb.py

diff --git a/scripts/follow_uwb.py b/scripts/follow_uwb.py
index cbc0be8f39bbca8b6540749380185f8570b8e223..6f6dd0d3b4f3a9bba87174fa6ececf9c4c0843d9 100755 (executable)
--- a/scripts/follow_uwb.py
+++ b/scripts/follow_uwb.py
@@ -8,6 +8,7 @@ import tf2_ros
 import dynamic_reconfigure.client
 import numpy as np
 import thread
+import threading
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 from actionlib_msgs.msg import GoalStatus
 from math import *
@@ -23,12 +24,24 @@ class FollowUWB:
 
                rospy.loginfo("Setting paramters")
                self.dynreconf = dynamic_reconfigure.client.Client("/move_base/TrajectoryPlannerROS")
-               #self.dynreconf.update_configuration({'max_vel_x': 1.0, 'max_vel_theta': 1.2, 'min_in_place_vel_theta': 1.0})
+               self.speed = None
+               self.__lock_set_dynreconf = threading.Lock()
+               self.set_speed("slow")
 
                rospy.loginfo("Waiting for the move_base action server to come up")
                self.move_base.wait_for_server()
                rospy.loginfo("Got move_base action server")
 
+       def set_speed(self, mode):
+               with self.__lock_set_dynreconf:
+                       if mode == "fast" and self.speed != "fast":
+                               print "speed", mode
+                               self.dynreconf.update_configuration({'max_vel_x': 1.0, 'max_vel_theta': 1.2, 'min_in_place_vel_theta': 1.0})
+                       elif mode == "slow" and self.speed != "slow":
+                               print "speed", mode
+                               self.dynreconf.update_configuration({'max_vel_x': 0.5, 'max_vel_theta': 1.0, 'min_in_place_vel_theta': 0.4})
+                       self.speed = mode
+
        def next_pos(self, x, y):
                rospy.loginfo("Moving to (%f, %f)" % (x, y))
 
@@ -51,11 +64,17 @@ class FollowUWB:
                        # Get the current position
                        pos = self.tfBuffer.lookup_transform("uwb_beacon", 'base_link', rospy.Time(0), rospy.Duration(2.0))
                        # Cancel if we are close enough to goal
-                       if np.linalg.norm([pos.transform.translation.y, pos.transform.translation.x]) < 1:
+                       diff = np.linalg.norm([pos.transform.translation.y, pos.transform.translation.x])
+                       if diff < 1:
                                rospy.loginfo("Goal within 1m, canceling")
                                self.move_base.cancel_goal()
                                return
 
+                       if diff > 5:
+                               self.set_speed("fast")
+                       elif diff < 3:
+                               self.set_speed("slow")
+
                if self.move_base.get_state() == GoalStatus.SUCCEEDED:
                        rospy.loginfo("The base moved to (%f, %f)" % (x, y))
                else:
@@ -68,6 +87,7 @@ class FollowUWB:
 
        def run(self):
                rate = rospy.Rate(2.0)
+               coords_last = (0, 0)
                while not rospy.is_shutdown():
                        # Get the current position
                        beacon_pos = self.tfBuffer.lookup_transform('base_link', "uwb_beacon", rospy.Time(0), rospy.Duration(2.0))
@@ -75,10 +95,11 @@ class FollowUWB:
                        coords_cur = (beacon_pos.transform.translation.x, beacon_pos.transform.translation.y)
 
                        # Check difference to last goal
-                       if np.linalg.norm(np.array(coords_cur)) > 0.5:
+                       if np.linalg.norm(np.array(coords_cur) - np.array(coords_last)) > 0.5:
                                print "Setting new goal"
                                # Set a new goal
                                thread.start_new_thread(self.next_pos, (coords_cur[0], coords_cur[1]))
+                               coords_last = coords_cur
                        rate.sleep()