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