# Wait for other quad to have taken off
tc.SetStatusSync(status=mc.QUAD_FLYING)
tc.WaitForStatusSync(partner="partner2", status=mc.QUAD_FLYING)
w4partner = tc.WaitForStatusSync(foreground=False,
partner="partner2",
status=mc.QUAD_CALLING)
tc.addCondition(ConditionIsCompleted("Partner2", tc, w4partner))
w4battery = tc.WaitForLowBattery(foreground=False, threshold=70.0)
tc.addCondition(ConditionIsCompleted("Battery", tc, w4battery))
try:
for i, p in enumerate(wp):
tc.GoTo(goal_x=p[0], goal_y=p[1], goal_z=p[2])
if p[3]:
tc.RadioScan(num_sectors=3, acquisition_time=2.0)
tc.SetStatusSync(status=mc.QUAD_DOWN)
tc.clearConditions()
except TaskConditionException, e:
cnames = [str(c) for c in e.conditions]
if "Partner2" in cnames:
# We've been called...
rospy.loginfo(
"Partner2 requested intervention, interrupting mission")
tc.GoTo(goal_x=p2.linear.x, goal_y=p2.linear.y, goal_z=1.0)
tc.RadioScan(num_sectors=3, acquisition_time=5.0)
tc.SetStatusSync(status=mc.QUAD_DONE_HELPING)
tc.GoTo(goal_x=wp[-1][0], goal_y=wp[-1][1], goal_z=wp[-1][2])
if "Battery" in cnames:
