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