2 # -*- coding: iso-8859-15 -*-
8 import dynamic_reconfigure.client
12 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
13 from actionlib_msgs.msg import GoalStatus
19 rospy.init_node('follow_uwb')
20 rospy.on_shutdown(self.on_shutdown)
23 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
24 self.tfBuffer = tf2_ros.Buffer()
25 self.listener = tf2_ros.TransformListener(self.tfBuffer)
27 rospy.loginfo("Setting paramters")
28 self.dynreconf = dynamic_reconfigure.client.Client("/move_base/TrajectoryPlannerROS")
30 self.__lock_set_dynreconf = threading.Lock()
31 self.set_speed("slow")
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")
37 def set_speed(self, mode):
38 with self.__lock_set_dynreconf:
39 if mode == "fast" and self.speed != "fast":
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":
44 self.dynreconf.update_configuration({'max_vel_x': 0.5, 'max_vel_theta': 1.0, 'min_in_place_vel_theta': 0.4})
47 def next_pos(self, x, y):
48 rospy.loginfo("Moving to (%f, %f)" % (x, y))
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)
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)
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()
76 self.set_speed("fast")
78 self.set_speed("slow")
80 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
81 rospy.loginfo("The base moved to (%f, %f)" % (x, y))
83 rospy.logerr("The base failed to (%f, %f). Error: %d" % (x, y, self.move_base.get_state()))
86 def on_shutdown(self):
87 rospy.loginfo("Canceling all goals")
88 self.move_base.cancel_all_goals()
91 rate = rospy.Rate(2.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])
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"
107 thread.start_new_thread(self.next_pos, (coords_cur[0], coords_cur[1]))
108 coords_last = coords_cur
112 if __name__ == "__main__":