def mission():
print("Starting mission...")
print("Paying attention to robots...")
mxc.startTask('PAY_ATTENTION_TO_ROBOT_MESSAGES')
print("informing position to robots...")
mxc.startTask('INFORM_POSITION_TO_ROBOTS')
print("Taking off...")
mxc.executeTask('TAKE_OFF')
print("Rotate...")
mxc.executeTask('ROTATE', relative_angle=180)
print("Following path...")
mxc.executeTask('FOLLOW_PATH', path=[[-1, 0, 2.3], [-1, 12, 2.3]])
print("Informing the other drone...")
mxc.executeTask('INFORM_ROBOTS',
receiver='drone112',
message='first_surface_inspection_completed')
print("Landing...")
mxc.executeTask('LAND')
print('Mission completed.')
def mission():
global qr_codes
print("Starting mission...")
print("Taking off...")
mxc.executeTask('TAKE_OFF')
mxc.startTask('HOVER')
print("Take off completed...")
j = 0
rospy.Subscriber("/drone111/all_beliefs", ListOfBeliefs, qr_callback)
uid = 0
mxc.startTask('CLEAR_OCCUPANCY_GRID')
for j, point in enumerate(points, 0):
retry = 0
print("Generating path")
print(str(point))
if (j == 1 or j == 3 or j == 5 or j == 9 or j == 11):
mxc.startTask('CLEAR_OCCUPANCY_GRID')
exit_code = 3
while (retry == 0 or exit_code == 3):
if (j == 2 or j == 4 or j == 8 or j == 10):
exit_code = mxc.executeTask('FOLLOW_PATH',
path=[points[j - 1], points[j]])[1]
retry = 1
else:
traject = mxc.executeTask('GENERATE_PATH', destination=point)
query = "path(?x,?y)"
success, unification = mxc.queryBelief(query)
print(query)
print(success)
if success:
x = str(unification['x'])
y = str(unification['y'])
predicate_path = "path(" + x + "," + y + ")"
mxc.removeBelief(predicate_path)
predicate_object = "object(" + x + ", path)"
mxc.removeBelief(predicate_object)
traject = eval(unification['y'])
traject = [[b for b in a] for a in traject]
i = 0
print("Moving to" + str(traject[len(traject) - 1]))
print len(traject)
print("Following path")
print("---------------------------------")
exit_code = mxc.executeTask('FOLLOW_PATH', path=traject)[1]
retry = 1
else:
print("next iteration")
mxc.startTask('CLEAR_OCCUPANCY_GRID')
if (j == 1):
mxc.startTask('PAY_ATTENTION_TO_QR_CODES')
mxc.executeTask('LAND')
#print(qr_codes)
#mxc.startTask('CLEAR_OCCUPANCY_GRID')
print('Finish mission...')
def mission():
print("Starting mission...")
print("TAKE_OFF...")
mxc.executeTask('TAKE_OFF')
print("ROTATE...")
mxc.executeTask('ROTATE', angle=60)
print("FOLLOW_PATH...")
mxc.executeTask('FOLLOW_PATH',
path=[[0, 20, 1], [20, 20, 1], [20, 0, 1], [0, 0, 1]])
print("LAND...")
mxc.executeTask('LAND')
print('Mission completed.')
def mission():
print("Starting mission...")
print("Taking off...")
mxc.executeTask('TAKE_OFF')
print("ROTATE...")
mxc.executeTask('ROTATE', relative_angle=60)
print("FOLLOW_PATH...")
mxc.executeTask('FOLLOW_PATH',
path=[[0, 2, 1], [2, 2, 1], [2, 0, 1], [0, 0, 1]])
print("Landing...")
mxc.executeTask('LAND')
print('Finish mission...')
def mission():
print("Starting mission...")
print("TAKE_OFF...")
mxc.executeTask('TAKE_OFF')
print("ROTATE...")
mxc.executeTask('ROTATE', relative_angle=270)
print("FOLLOW_PATH...")
mxc.executeTask('FOLLOW_PATH',
path=[[0, 2, 1], [2, 2, 1], [2, 0, 1], [0, 0, 1]])
print('Mission completed.')
def mission():
print("Starting mission...")
print("Taking off...")
mxc.executeTask('TAKE_OFF_WITH_DF')
#print("FOLLOW_PATH...")
mxc.startTask('FOLLOW_PATH_WITH_DF')
mxc.executeTask('SEND_PATH', path = [ [1,1,5], [2,4,1]])
# time.sleep(10)
print("SEND PATH...")
mxc.executeTask('LAND_WITH_DF')
print('Finish mission...')
def mission():
print("Starting mission...")
print("Taking off...")
mxc.executeTask('TAKE_OFF')
print("FOLLOW_PATH...")
mxc.executeTask('FOLLOW_PATH',
path=[[1, -1, 1], [1, 1, 1], [-1, 1, 1], [-1, -1, 1],
[0, 0, 1]])
print("Landing...")
mxc.executeTask('LAND')
print('Finish mission...')
def mission():
global qr_codes
print("Starting mission...")
# api.executeBehavior('CLEAR_OCCUPANCY_GRID')
print("Taking off...")
mxc.executeTask('TAKE_OFF')
mxc.startTask('HOVER')
mxc.startTask('CLEAR_OCCUPANCY_GRID')
print("Take off completed...")
floor = 0
for j, point in enumerate(points, 0):
retry = 0
exit_code = 3
while (retry == 0 or exit_code == 3):
if (j == 0 or j == 3 or j == 6 or j == 9):
print("Follow path simple")
exit_code = mxc.executeTask('FOLLOW_PATH', path=[points[j]])[1]
mxc.startTask('CLEAR_OCCUPANCY_GRID')
retry = 1
time.sleep(5)
else:
print("Generating path")
print(str(point))
traject = mxc.executeTask('GENERATE_PATH', destination=point)
query = "path(?x,?y)"
success, unification = mxc.queryBelief(query)
if success:
x = str(unification['x'])
y = str(unification['y'])
predicate_path = "path(" + x + "," + y + ")"
mxc.removeBelief(predicate_path)
predicate_object = "object(" + x + ", path)"
mxc.removeBelief(predicate_object)
traject = eval(unification['y'])
traject = [[b for b in a] for a in traject]
print("Moving to" + str(traject[len(traject) - 1]))
print len(traject)
print("Following path")
print("---------------------------------")
print(j)
exit_code = mxc.executeTask('FOLLOW_PATH', path=traject)[1]
if (j == 1 or j == 4 or j == 7 or j == 10):
if (exit_code != 3):
print(j)
mxc.startTask(
'SAVE_OCCUPANCY_GRID',
map_name="$APPLICATION_PATH/maps/planta" +
str(floor))
floor = floor + 1
if (j == 1):
mxc.startTask('CLEAR_OCCUPANCY_GRID')
retry = 1
if (exit_code == 3):
mxc.startTask('CLEAR_OCCUPANCY_GRID')
retry = 0
else:
print("next iteration")
mxc.startTask('CLEAR_OCCUPANCY_GRID')
result = mxc.executeTask('LAND')
mxc.executeTask('CLEAR_OCCUPANCY_GRID')
print('Finish mission...')
def mission():
last_code = "INIT"
print("Starting mission...")
print("TAKE_OFF...")
mxc.executeTask('TAKE_OFF')
print("PAY_ATTENTION_TO_QR_CODES...")
mxc.startTask('PAY_ATTENTION_TO_QR_CODES')
while (True):
s = 0
s, num = mxc.queryBelief('object(?N,qr_code)')
s, message = mxc.queryBelief('object(qr,?L)')
if s:
executed_action = False
t0 = time.clock()
while (not executed_action):
if message['L'] == "CW90" and (last_code != message['L'] or
(time.clock() - t0) > 5):
executed_action = True
print("Received 'ROTATE CLOCKWISE 90'")
mxc.executeTask('ROTATE', relative_angle=90)
last_code = message['L']
mxc.removeBelief('object(qr,' + message['L'] + ')')
mxc.removeBelief('object(' + str(num['N']) + ',' +
message['L'] + ')')
mxc.removeBelief('code(' + str(num['N']) + ',' +
message['L'] + ')')
if message['L'] == "CCW90" and (last_code != message['L'] or
(time.clock() - t0) > 5):
executed_action = True
print("Received 'ROTATE COUNTERCLOCKWISE 90'")
mxc.executeTask('ROTATE', relative_angle=90)
last_code = message['L']
mxc.removeBelief('object(qr,' + message['L'] + ')')
mxc.removeBelief('object(' + str(num['N']) + ',' +
message['L'] + ')')
mxc.removeBelief('code(' + str(num['N']) + ',' +
message['L'] + ')')
if message['L'] == "DOWN" and (last_code != message['L'] or
(time.clock() - t0) > 5):
executed_action = True
print("Received 'DOWN'")
mxc.executeTask('MOVE_VERTICAL', distance=-1)
last_code = message['L']
mxc.removeBelief('object(qr,' + message['L'] + ')')
mxc.removeBelief('object(' + str(num['N']) + ',' +
message['L'] + ')')
mxc.removeBelief('code(' + str(num['N']) + ',' +
message['L'] + ')')
if message['L'] == "UP" and (last_code != message['L'] or
(time.clock() - t0) > 5):
executed_action = True
print("Received 'UP'")
mxc.executeTask('MOVE_VERTICAL', distance=1)
last_code = message['L']
mxc.removeBelief('object(qr,' + message['L'] + ')')
mxc.removeBelief('object(' + str(num['N']) + ',' +
message['L'] + ')')
mxc.removeBelief('code(' + str(num['N']) + ',' +
message['L'] + ')')
if message['L'] == "LAND" and (last_code != message['L'] or
(time.clock() - t0) > 5):
executed_action = True
print("Received 'LAND'")
mxc.executeTask('LAND')
last_code = message['L']
mxc.removeBelief('object(qr,' + message['L'] + ')')
mxc.removeBelief('object(' + str(num['N']) + ',' +
message['L'] + ')')
mxc.removeBelief('code(' + str(num['N']) + ',' +
message['L'] + ')')
if message['L'] == "MOVE_LEFT" and (last_code != message['L']
or
(time.clock() - t0) > 5):
executed_action = True
print("Received 'MOVE_LEFT'")
mxc.startTask('MOVE_AT_SPEED',
direction="LEFT",
speed=0.15)
last_code = message['L']
mxc.removeBelief('object(qr,' + message['L'] + ')')
mxc.removeBelief('object(' + str(num['N']) + ',' +
message['L'] + ')')
mxc.removeBelief('code(' + str(num['N']) + ',' +
message['L'] + ')')
if message['L'] == "MOVE_RIGHT" and (last_code != message['L']
or
(time.clock() - t0) > 5):
executed_action = True
print("Received 'MOVE_RIGHT'")
mxc.startTask('MOVE_AT_SPEED',
direction="RIGHT",
speed=0.15)
last_code = message['L']
mxc.removeBelief('object(qr,' + message['L'] + ')')
mxc.removeBelief('object(' + str(num['N']) + ',' +
message['L'] + ')')
mxc.removeBelief('code(' + str(num['N']) + ',' +
message['L'] + ')')
