def robot_perform_task(robot_ip, task):
# send new route to the robot
robot_pos = get_robot_pos(robot_ip)
if task.type == TaskType.TO_DOCK:
# send robot the route it needs to trace
while True:
route = route_corridor_to_dock(robot_pos, task.dest_dock_id)
print("robot", robot_ip, "at", robot_pos, task, route)
message = pack("B" * 6, 0, 1, robot_pos.from_x, robot_pos.from_y, robot_pos.to_x, robot_pos.to_y)
message += compile_route(route)
robot_pos = send_route(robot_ip, message)
if not robot_pos:
break
update_robot_pos(robot_ip, robot_pos.from_x, robot_pos.from_y, robot_pos.to_x, robot_pos.to_y)
# update bookkeeping
db_manager.set_robot_dest_dock(robot_ip, task.dest_dock_id)
else: # task.type == planning.TaskType.FOR_CONTAINER
# get info about the container
(container_id, x, y, slot, level, status) = db_manager.get_container_info(task.dest_container_id)
if status == "ON_SHELF":
# send new route to the robot
while True:
(route, last_road_orientation) = route_corridor_to_shelf(robot_pos, ShelfLoc(x, y, slot, level))
print("robot", robot_ip, "at", robot_pos, task, route)
message = compile_to_shelf_message(robot_pos, route, last_road_orientation, x, y, slot, level, False)
robot_pos = send_route(robot_ip, message)
if not robot_pos:
break
update_robot_pos(robot_ip, robot_pos.from_x, robot_pos.from_y, robot_pos.to_x, robot_pos.to_y)
# update bookkeeping
db_manager.set_robot_dest_dock(robot_ip, task.dest_dock_id)
db_manager.set_robot_dest_shelf(robot_ip, x, y, slot, level)
db_manager.set_robot_container(robot_ip, task.dest_container_id)
db_manager.set_container_status(container_id, "RESERVED")
else:
raise Exception("robot_perform_task: Container #", container_id, " is not on shelf. Cannot fetch")
def dismiss_robot(conn, addr):
container_id = unpack("B", receive_message(conn, 1))[0]
dock_id = db_manager.get_dock_id_by_ip(addr[0])
# find where the grasper for the robot is
is_grasper_right = db_manager.is_on_dock_robot_grasper_on_right(dock_id)
# decouple the robot from the dock
robot_ip = db_manager.robot_leave_dock(dock_id)
# tell the robot which container it is carrying
# if the container has already been taken away, mark the robot free
container_exist = db_manager.set_robot_container(robot_ip, container_id)
if not container_exist:
assigned_task = planning.add_free_robot(robot_ip)
if assigned_task is None:
planning.robot_go_idle(robot_ip)
else:
planning.robot_perform_task(robot_ip, assigned_task)
return
# otherwise
# find the nearest empty shelf slot for the robot to put its container
# send the route to this robot
(x, y, slot, level) = planning.nearest_empty_shelf_slot(dock_id, is_grasper_right)
(route, last_road_orientation) = planning.route_dock_to_shelf(dock_id, ShelfLoc(x, y, slot, level))
print("robot", robot_ip, "dismissed to", ShelfLoc(x, y, slot, level), "en route", route)
if dock_id == 1:
robot_pos = CorLoc(1, 0, 2, 0)
elif dock_id == 2:
robot_pos = CorLoc(2, 0, 3, 0)
else:
raise Exception("dismiss_robot: robot", robot_ip, "dismissed from unknown dock #", dock_id)
message = planning.compile_to_shelf_message(robot_pos, route, last_road_orientation, x, y, slot, level, True)
status = planning.send_route(robot_ip, message)
# the robot cannot possibly have moved
assert not status, "dismiss_robot: robot has moved, impossible case"
# update bookkeeping
db_manager.set_robot_dest_shelf(robot_ip, x, y, slot, level)
db_manager.update_container_dest(container_id, x, y, slot, level)
# TODO: if PACKING, tell worker app to disable "dismiss" button (maybe not?)
print("Robot " + addr[0] + " is dismissed")