scripts/follow_uwb.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3
   4 import rospy
   5 import tf
   6 import actionlib
   7 import tf2_ros
   8 import dynamic_reconfigure.client
   9 import numpy as np
  10 import thread
  11 import threading
  12 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
  13 from actionlib_msgs.msg import GoalStatus
  14 from math import *
  15
  16
  17 class FollowUWB:
  18         def __init__(self):
  19                 rospy.init_node('follow_uwb')
  20                 rospy.on_shutdown(self.on_shutdown)
  21                 self.dist_stop = 1.0
  22
  23                 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
  24                 self.tfBuffer = tf2_ros.Buffer()
  25                 self.listener = tf2_ros.TransformListener(self.tfBuffer)
  26
  27                 rospy.loginfo("Setting paramters")
  28                 self.dynreconf = dynamic_reconfigure.client.Client("/move_base/TrajectoryPlannerROS")
  29                 self.speed = None
  30                 self.__lock_set_dynreconf = threading.Lock()
  31                 self.set_speed("slow")
  32
  33                 rospy.loginfo("Waiting for the move_base action server to come up")
  34                 self.move_base.wait_for_server()
  35                 rospy.loginfo("Got move_base action server")
  36
  37         def set_speed(self, mode):
  38                 with self.__lock_set_dynreconf:
  39                         if mode == "fast" and self.speed != "fast":
  40                                 print "speed", mode
  41                                 self.dynreconf.update_configuration({'max_vel_x': 1.0, 'max_vel_theta': 1.2, 'min_in_place_vel_theta': 1.0})
  42                         elif mode == "slow" and self.speed != "slow":
  43                                 print "speed", mode
  44                                 self.dynreconf.update_configuration({'max_vel_x': 0.5, 'max_vel_theta': 1.0, 'min_in_place_vel_theta': 0.4})
  45                         self.speed = mode
  46
  47         def next_pos(self, x, y):
  48                 rospy.loginfo("Moving to (%f, %f)" % (x, y))
  49
  50                 # calculate angle between current position and goal
  51                 angle_to_goal = atan2(y, x)
  52                 odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle_to_goal)
  53
  54                 goal = MoveBaseGoal()
  55                 goal.target_pose.header.frame_id = "base_link"
  56                 goal.target_pose.header.stamp = rospy.Time.now()
  57                 goal.target_pose.pose.position.x = x
  58                 goal.target_pose.pose.position.y = y
  59                 goal.target_pose.pose.orientation.x = odom_quat[0]
  60                 goal.target_pose.pose.orientation.y = odom_quat[1]
  61                 goal.target_pose.pose.orientation.z = odom_quat[2]
  62                 goal.target_pose.pose.orientation.w = odom_quat[3]
  63                 self.move_base.send_goal(goal)
  64
  65                 while not self.move_base.wait_for_result(rospy.Duration(1.0)) and not rospy.is_shutdown():
  66                         # Get the current position
  67                         pos = self.tfBuffer.lookup_transform("uwb_beacon", 'base_link', rospy.Time(0), rospy.Duration(2.0))
  68                         # Cancel if we are close enough to goal
  69                         diff = np.linalg.norm([pos.transform.translation.y, pos.transform.translation.x])
  70                         if diff < self.dist_stop:
  71                                 rospy.loginfo("Goal within %0fm, canceling", self.dist_stop)
  72                                 self.move_base.cancel_goal()
  73                                 return
  74
  75                         if diff > 5:
  76                                 self.set_speed("fast")
  77                         elif diff < 3:
  78                                 self.set_speed("slow")
  79
  80                 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
  81                         rospy.loginfo("The base moved to (%f, %f)" % (x, y))
  82                 else:
  83                         rospy.logerr("The base failed to (%f, %f). Error: %d" % (x, y, self.move_base.get_state()))
  84                         exit(1)
  85
  86         def on_shutdown(self):
  87                 rospy.loginfo("Canceling all goals")
  88                 self.move_base.cancel_all_goals()
  89
  90         def run(self):
  91                 rate = rospy.Rate(2.0)
  92                 coords_last = (0, 0)
  93                 angle_last = 0
  94                 while not rospy.is_shutdown():
  95                         # Get the current position
  96                         beacon_pos = self.tfBuffer.lookup_transform('base_link', "uwb_beacon", rospy.Time(0), rospy.Duration(2.0))
  97                         # Create a position 1m away by creating an inverse vector
  98                         coords_cur = (beacon_pos.transform.translation.x, beacon_pos.transform.translation.y)
  99                         angle_cur = atan2(coords_cur[1], coords_cur[0])
 100
 101                         # Check difference to last goal
 102                         if ((np.linalg.norm(np.array(coords_cur) - np.array(coords_last)) > 0.5
 103                             and np.linalg.norm(np.array(coords_cur) > self.dist_stop))
 104                             or abs(angle_cur - angle_last) > 1.0):
 105                                 print "Setting new goal"
 106                                 # Set a new goal
 107                                 thread.start_new_thread(self.next_pos, (coords_cur[0], coords_cur[1]))
 108                                 coords_last = coords_cur
 109                         rate.sleep()
 110
 111
 112 if __name__ == "__main__":
 113         p = FollowUWB()
 114         p.run()