ugv.GoTo(goal_x=p[0],goal_y=p[1],max_velocity=maxVel)
ugv.Wait(duration=1.0)
if (i < 4):
i+=1
else:
i=0
except TaskConditionException, e:
if ugv.isCompleted(w4takeOffReq):
rospy.loginfo("Mission suspended - Take-Off Requested !")
ugv.WaitingForTakeOff()
if ugv.isCompleted(w4landingReq):
rospy.loginfo("Mission suspended - Landing Requested !")
ugv.WaitingForLanding()
if ugv.isCompleted(w4interruption):
rospy.loginfo("Mission suspended - Interruption Requested !")
ugv.OrderLandingBack()
ugv.WaitForLandingRequest()
ugv.WaitingForLanding()
ugv.WaitingForRecovery()
ugv.OrderTakeOff()
ugv.WaitForTakeOffRequest()
ugv.WaitingForTakeOff()
except TaskException, e:
rospy.logerr("Exception caught: " + str(e))
if not rospy.core.is_shutdown():
ugv.SetManual()
