def add_free_robot(robot_ip):
with free_list_lock, pending_lock:
i = 0
while i < len(pending_tasks):
task = pending_tasks[i]
if task.type == TaskType.FOR_CONTAINER:
(container_id, row, col, slot, level, status) = db_manager.get_container_info(task.dest_container_id)
# if the task is a FETCHING job where the requested container is TO_SHELF,
# re-assign task to that robot TO_PACKING
if status == "TO_SHELF":
carrier_robot_ip = db_manager.get_container_carrier(task.dest_container_id)
robot_perform_task(carrier_robot_ip, Task(TaskType.TO_DOCK, task.dest_dock_id, 0))
# update bookkeeping
db_manager.set_container_status(container_id, "TO_PACKING")
pending_tasks.pop(i)
continue
# if the task is a FETCHING job where the requested container is TO_IMPORT,
# re-assign task to that robot TO_PACKING
# and let this newly added robot to the TO_IMPORT instead
elif status == "TO_IMPORT":
carrier_robot_ip = db_manager.get_container_carrier(task.dest_container_id)
destined_import_dock_id = db_manager.get_robot_dest_dock(carrier_robot_ip)
robot_perform_task(carrier_robot_ip, Task(TaskType.TO_DOCK, task.dest_dock_id, 0))
db_manager.set_container_status(container_id, "TO_PACKING")
pending_tasks.pop(i)
return Task(TaskType.TO_DOCK, destined_import_dock_id, 0)
# if the task is a FETCHING job where the requested container is RESERVED or TO_PACKING,
# it cannot be done now, skip, try the next task
elif status == "RESERVED" or status == "TO_PACKING":
i += 1
continue
# otherwise, the task is do-able, assign it to this newly added robot
pending_tasks.pop(i)
return task
# if there's no task can be done, add the robot to free list
free_robots.append(robot_ip)
return None
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")
