class make_coords: def __init__(self, x, y): # Sets the value of myCoords[box_number] self.x = x self.y = y # Initialize our data structure to store our x,y coordinates for each obstacle: myCoords = {} # Read from the .csv file: dataFile = 'custom_world_coords.csv' rawData = parseCSVstring(dataFile, returnJagged=True, fillerValue=-1, delimiter=',', commentChar='%') for i in range(0, len(rawData)): x = int(rawData[i][0]) y = int(rawData[i][1]) myCoords[i + 1] = make_coords(x, y) # Open the outfile for writing: outFile = open("custom_world.world", 'w') preamble = """ <sdf version='1.4'> <world name='default'> <light name='sun' type='directional'> <cast_shadows>1</cast_shadows>
def readData(self, numUAVs): # b) tbl_vehicles.csv tmpUAVs = 0 rawData = parseCSVstring(self.vehiclesFile, returnJagged=False, fillerValue=-1, delimiter=',', commentChar='%') for i in range(0,len(rawData)): vehicleID = int(rawData[i][0]) vehicleType = int(rawData[i][1]) takeoffSpeed = float(rawData[i][2]) cruiseSpeed = float(rawData[i][3]) landingSpeed = float(rawData[i][4]) yawRateDeg = float(rawData[i][5]) cruiseAlt = float(rawData[i][6]) capacityLbs = float(rawData[i][7]) launchTime = float(rawData[i][8]) # [seconds]. recoveryTime = float(rawData[i][9]) # [seconds]. serviceTime = float(rawData[i][10]) # [seconds]. batteryPower = float(rawData[i][11]) # [joules]. flightRange = str(rawData[i][12]) # 'high' or 'low' if (vehicleType == TYPE_UAV): tmpUAVs += 1 if (tmpUAVs <= numUAVs): self.vehicle[vehicleID] = make_vehicle(vehicleType, takeoffSpeed, cruiseSpeed, landingSpeed, yawRateDeg, cruiseAlt, capacityLbs, launchTime, recoveryTime, serviceTime, batteryPower, flightRange) else: self.vehicle[vehicleID] = make_vehicle(vehicleType, takeoffSpeed, cruiseSpeed, landingSpeed, yawRateDeg, cruiseAlt, capacityLbs, launchTime, recoveryTime, serviceTime, batteryPower, flightRange) if (tmpUAVs < numUAVs): print("WARNING: You requested %d UAVs, but we only have data on %d UAVs." % (numUAVs, tmpUAVs)) print("\t We'll solve the problem with %d UAVs. Sorry." % (tmpUAVs)) # a) tbl_locations.csv rawData = parseCSVstring(self.locationsFile, returnJagged=False, fillerValue=-1, delimiter=',', commentChar='%') for i in range(0,len(rawData)): nodeID = int(rawData[i][0]) nodeType = int(rawData[i][1]) latDeg = float(rawData[i][2]) # IN DEGREES lonDeg = float(rawData[i][3]) # IN DEGREES altMeters = float(rawData[i][4]) parcelWtLbs = float(rawData[i][5]) if (len(rawData[i]) == 10): addressStreet = str(rawData[i][6]) addressCity = str(rawData[i][7]) addressST = str(rawData[i][8]) addressZip = str(rawData[i][9]) address = '%s, %s, %s, %s' % (addressStreet, addressCity, addressST, addressZip) else: address = '' # or None? serviceTimeTruck = self.vehicle[1].serviceTime if numUAVs > 0: serviceTimeUAV = self.vehicle[2].serviceTime else: serviceTimeUAV = 0 self.node[nodeID] = make_node(nodeType, latDeg, lonDeg, altMeters, parcelWtLbs, serviceTimeTruck, serviceTimeUAV, address) # c) tbl_truck_travel_data.csv if (os.path.isfile(self.distmatrixFile)): # Travel matrix file exists rawData = parseCSV(self.distmatrixFile, returnJagged=False, fillerValue=-1, delimiter=',') for i in range(0,len(rawData)): tmpi = int(rawData[i][0]) tmpj = int(rawData[i][1]) tmpTime = float(rawData[i][2]) tmpDist = float(rawData[i][3]) for vehicleID in self.vehicle: if (self.vehicle[vehicleID].vehicleType == TYPE_TRUCK): self.travel[vehicleID][tmpi][tmpj] = make_travel(0.0, tmpTime, 0.0, tmpTime, 0.0, tmpDist, 0.0, tmpDist) else: # Travel matrix file does not exist print("ERROR: Truck travel data is not available. Please provide a data matrix in the following format in a CSV file, and try again:\n") print("from node ID | to node ID | travel time [seconds] | travel distance [meters]\n") exit()
village_sod_D= defaultdict(make_dict) village_sod_P= defaultdict(make_dict) village_cpd_M= defaultdict(make_dict) village_cpd_D= defaultdict(make_dict) village_cpd_P= defaultdict(make_dict) village_visited = defaultdict(make_dict) #sod_count_M = defaultdict(make_dict) depot_lat_deg = 0.40 depot_long_deg = 32.48 village_SSI=defaultdict(make_dict) village_data = parseCSVstring(input_village_data, returnJagged=False, fillerValue=-1, delimiter=',') for i in range(1,len(village_data)): if (len(village_data[i]) == 16): tmp_ID = int(village_data[i][0]) tmp_long_deg = float(village_data[i][1]) tmp_lat_deg = float(village_data[i][2]) tmp_D1 = int(village_data[i][3]) tmp_D2 = int(village_data[i][4]) tmp_D3 = int(village_data[i][5]) tmp_E = int(village_data[i][6]) tmp_ap_M = float(village_data[i][7]) tmp_ap_D = float(village_data[i][8]) tmp_ap_P = float(village_data[i][9]) tmp_sod_M = float(village_data[i][10]) tmp_sod_D = float(village_data[i][11]) tmp_sod_P = float(village_data[i][12])
def __init__(self): # Need the user to specify the game to play: if (len(sys.argv) == 2): thisRace = str(sys.argv[1]) print "Starting the game using the %s track" % (thisRace) else: print "You must provide a track name. Sorry things didn't work out. Bye." exit() # Capture the parameters for this track: self.init_x = {} self.init_y = {} trackFile = "races/race_params_%s.txt" % (thisRace) try: rawInfo = parseCSVstring(trackFile, returnJagged=True, fillerValue=-1, delimiter=',') # Convert all values to floats for i in range(0, len(rawInfo)): rawInfo[i] = map(float, rawInfo[i]) # 1) Verify the values in the race params file: # a) 10 x's if (len(rawInfo[0]) != 10): print "Sorry, you need to provide exactly 10 x coordinates for the robot starting positions." exit() # b) 10 y's if (len(rawInfo[1]) != 10): print "Sorry, you need to provide exactly 10 y coordinates for the robot starting positions." exit() # c) either all x's are the same or all y's are the same. if ((min(rawInfo[0]) != max(rawInfo[0])) and (min(rawInfo[1]) != max(rawInfo[1]))): print "Sorry, either all of the x or all of the y coordinates need to be the same." exit() # d) finish rectangle is properly defined. if (len(rawInfo[2]) != 2): print "Sorry, you need to provide 2 values for the x coordinates of the finish rectangle." exit() if (len(rawInfo[3]) != 2): print "Sorry, you need to provide 2 values for the y coordinates of the finish rectangle." exit() if (rawInfo[2][0] >= rawInfo[2][1]): print "Sorry, the max x value of the finish rectangle must be larger than the min x value." exit() if (rawInfo[3][0] >= rawInfo[3][1]): print "Sorry, the max y value of the finish rectangle must be larger than the min y value." exit() # 2) Get the x and y coordinates for 10 robots. # This info is in the first two lines of the file (rows 0 and 1, columns 0 through 9). for j in range(1, 10 + 1): self.init_x[j] = float(rawInfo[0][j - 1]) self.init_y[j] = float(rawInfo[1][j - 1]) # 3) Get the coordinates for the finish "rectangle". # This infor is in the 3rd and 4th lines of the file (rows 2 and 3) self.MIN_X = float(rawInfo[2][0]) self.MAX_X = float(rawInfo[2][1]) self.MIN_Y = float(rawInfo[3][0]) self.MAX_Y = float(rawInfo[3][1]) except: print "There was an error reading the %s track file. Perhaps this file doesn't exist?" % ( trackFile) exit() # In ROS, nodes are uniquely named. If two nodes with the same # node are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. rospy.init_node('listener', anonymous=True) rate = rospy.Rate(10) # Is 10 Hz fast enough? rospy.Subscriber("telem", telem, self.telemCallback) # Initialize our robotID counter: self.nextID = 0 # Initialize our turtle data structure self.turtle = {} # An empty Python dictionary # Initialize the race to be unstarted self.isRaceStarted = False # Define the publisher that will initiate the race: self.pub_start_race = rospy.Publisher('start_race', start_race, queue_size=10) # Define the service that will provide ID numbers: print "Initializing 'get_id' service...", self.id_service = rospy.Service('get_id', get_id, self.get_id_callback) print "DONE" # WAIT HERE FOR THE USER TO RELEASE THE GAME: print '*****HIT ENTER TO RELEASE THE GAME*****' raw_input() print 'HERE WE GO...' self.isRaceStarted = True # Establish the start time of the race: self.startTime = rospy.get_time() # Publish a message so our turtlebots will start the race. start_msg = start_race() start_msg.goal_x = [self.MIN_X, self.MAX_X] start_msg.goal_y = [self.MIN_Y, self.MAX_Y] self.pub_start_race.publish(start_msg) # spin() simply keeps python from exiting until this node is stopped rospy.spin()
# matrices that store the items that each picker and transporter should pick picker_matrix = np.linspace(0,0,item_qty*picker_qty).reshape(picker_qty,item_qty).astype(int) trans_matrix = np.linspace(0,0,item_qty*trans_qty).reshape(trans_qty,item_qty).astype(int) # slot index is from 0 to slot_qty-1. for i in range(picker_qty): picker_initial = random.choice(item_list) item_list.remove(picker_initial) picker_matrix[i,0] = picker_initial # get the warehouse distrance matrix Distance_Matrix_File = '../testdata/distance.csv' rawData = parseCSVstring(Distance_Matrix_File, returnJagged=True, fillerValue=-1, delimiter=',', commentChar='%') # The list of expected travel distance for each picker [picker1,picker2,...,pickern] and each transporter [trans1,trans2,...transn] picker_expect = np.linspace(0,0,picker_qty) trans_expect = np.linspace(0,0,trans_qty) # build item list for each picker while (len(item_list)>0): minimal = float('inf') # find the next item and its picker for i in item_list: for j in range(picker_qty): current = np.max(np.nonzero(picker_matrix[j,:])) start = picker_matrix[j,current] new_tra = float(rawData[i][start])+ float(picker_expect[j])