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")