From 6064553e6895205dd17bb5a49914415b24623de7 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Wed, 5 Sep 2018 22:23:16 +0200 Subject: [PATCH] follow_uwb: check angle changes --- scripts/follow_uwb.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) 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])) -- 2.39.5