From: Erik Andresen <erik@vontaene.de>
Date: Wed, 5 Sep 2018 20:23:16 +0000 (+0200)
Subject: follow_uwb: check angle changes
X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=6064553e6895205dd17bb5a49914415b24623de7;p=ros_wild_thumper.git

follow_uwb: check angle changes
---

diff --git a/scripts/follow_uwb.py b/scripts/follow_uwb.py
index 6f6dd0d..e6c5b1e 100755
--- a/scripts/follow_uwb.py
+++ b/scripts/follow_uwb.py
@@ -13,10 +13,12 @@ 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()
@@ -65,8 +67,8 @@ class FollowUWB:
 			pos = self.tfBuffer.lookup_transform("uwb_beacon", 'base_link', rospy.Time(0), rospy.Duration(2.0))
 			# Cancel if we are close enough to goal
 			diff = np.linalg.norm([pos.transform.translation.y, pos.transform.translation.x])
-			if diff < 1:
-				rospy.loginfo("Goal within 1m, canceling")
+			if diff < self.dist_stop:
+				rospy.loginfo("Goal within %0fm, canceling", self.dist_stop)
 				self.move_base.cancel_goal()
 				return
 
@@ -88,14 +90,18 @@ 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) - np.array(coords_last)) > 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]))