follow_uwb: check angle changes

diff --git a/scripts/follow_uwb.py b/scripts/follow_uwb.py
index 6f6dd0d3b4f3a9bba87174fa6ececf9c4c0843d9..e6c5b1e9791edc823f49f39fcd90a73f50448f00 100755 (executable)
--- 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]))