X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Ffollow_uwb.py;h=e6c5b1e9791edc823f49f39fcd90a73f50448f00;hp=cbc0be8f39bbca8b6540749380185f8570b8e223;hb=2f194b3fe82009a77b79021f0c57e6ca55c1706f;hpb=78385af76591e5fa2c228f7e53c3b7f2fc2092d6

diff --git a/scripts/follow_uwb.py b/scripts/follow_uwb.py
index cbc0be8..e6c5b1e 100755
--- 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()