www: properly set image hostname

[ros_wild_thumper.git] / scripts / follow_uwb.py

diff --git a/scripts/follow_uwb.py b/scripts/follow_uwb.py
index cbc0be8f39bbca8b6540749380185f8570b8e223..e6c5b1e9791edc823f49f39fcd90a73f50448f00 100755 (executable)
--- a/scripts/follow_uwb.py
+++ b/scripts/follow_uwb.py
@@ -8,14 +8,17 @@ 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 *
 
+
 class FollowUWB:
        def __init__(self):
                rospy.init_node('follow_uwb')
                rospy.on_shutdown(self.on_shutdown)
+               self.dist_stop = 1.0
 
                self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
                self.tfBuffer = tf2_ros.Buffer()
@@ -23,12 +26,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 +66,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:
-                               rospy.loginfo("Goal within 1m, canceling")
+                       diff = np.linalg.norm([pos.transform.translation.y, pos.transform.translation.x])
+                       if diff < self.dist_stop:
+                               rospy.loginfo("Goal within %0fm, canceling", self.dist_stop)
                                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,17 +89,23 @@ class FollowUWB:
 
        def run(self):
                rate = rospy.Rate(2.0)
+               coords_last = (0, 0)
+               angle_last = 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))
                        # Create a position 1m away by creating an inverse vector
                        coords_cur = (beacon_pos.transform.translation.x, beacon_pos.transform.translation.y)
+                       angle_cur = atan2(coords_cur[1], coords_cur[0])
 
                        # 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
+                           and np.linalg.norm(np.array(coords_cur) > self.dist_stop))
+                           or abs(angle_cur - angle_last) > 1.0):
                                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()