From 3c14550f0f80e0a22777e62036e7b0c4209baa5a Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Wed, 4 Jul 2018 14:26:32 +0200 Subject: [PATCH] follow_uwb: Variable speed dependant on distance to target --- scripts/follow_uwb.py | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/scripts/follow_uwb.py b/scripts/follow_uwb.py index cbc0be8..6f6dd0d 100755 --- a/scripts/follow_uwb.py +++ b/scripts/follow_uwb.py @@ -8,6 +8,7 @@ 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 * @@ -23,12 +24,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 +64,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: + diff = np.linalg.norm([pos.transform.translation.y, pos.transform.translation.x]) + if diff < 1: rospy.loginfo("Goal within 1m, canceling") 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,6 +87,7 @@ class FollowUWB: def run(self): rate = rospy.Rate(2.0) + coords_last = (0, 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)) @@ -75,10 +95,11 @@ class FollowUWB: coords_cur = (beacon_pos.transform.translation.x, beacon_pos.transform.translation.y) # 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: 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() -- 2.39.5