+ while not self.move_base.wait_for_result(rospy.Duration(1.0)) and not rospy.is_shutdown():
+ # Get the current position
+ pos = self.tfBuffer.lookup_transform("utm", 'base_link', rospy.Time(0), rospy.Duration(2.0))
+ # Cancel if we are close enough to goal
+ if np.linalg.norm([point.northing - pos.transform.translation.y, point.easting - pos.transform.translation.x]) < 1:
+ rospy.loginfo("Goal within 1m, canceling")
+ self.move_base.cancel_goal()
+ return