self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.tfBuffer = tf2_ros.Buffer()
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.tfBuffer = tf2_ros.Buffer()
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])
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)
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)
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)
- 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):