def __init__(self, image_Address, dbLocation, botRadius, botMaxVelocity, botturningVelocity, RSS0, nakagamiA, nakagamiB, scale_factor, timeStep):
#self.gui = guiStart()
self.dbHelper = databaseManager(dbLocation)
self.input_image = image_Address
self.scale_factor = scale_factor
self.timeStep = timeStep
self.apLocations = self.dbHelper.getDbEntry('L1')
self.legalPoints = self.dbHelper.getDbEntry('L2')
self.fingerprintDict = self.dbHelper.getDbEntry('L3')
self.mapParametersDict = self.dbHelper.getDbEntry('L4')
self.rssFingerprintDict = self.dbHelper.getDbEntry('L5')
self.locactionServicesHelper = locationServices(self.rssFingerprintDict,self.apLocations,self.legalPoints,RSS0,nakagamiA,nakagamiB)
self.pathPlanner = planPath(self.input_image,'Database/runtimeImages/path.jpg',self.legalPoints,self.scale_factor)
self.pathTracker = pathTracker()
self.magnificationServiceHelper = magnificationService(self.input_image,self.scale_factor)
self.botRadius =botRadius
self.botMaxVelocity = botMaxVelocity
self.botturningVelocity = botturningVelocity
self.vL = 0
self.vR = 0
self.timeStepCount =0
self.currentWP = 0
self.img = cv2.imread(self.input_image,0)
self.img = cv2.cvtColor(self.img,cv2.COLOR_GRAY2RGB)
self.img = cv2.resize(self.img,(int(self.img.shape[1]*self.scale_factor),int(self.img.shape[0]*self.scale_factor)))
def gen_waypoints(passed_in_arguments):
# Read in arguments:
argboundaryVertices = np.asarray(passed_in_arguments['boundaryVerts'])
arghotSpotsIn = np.asarray(passed_in_arguments['hotSpots'])
arghotSpots = sortWesternPoints(arghotSpotsIn)
argPVal = passed_in_arguments['pSliderVal']
argGridVal = passed_in_arguments['gridSliderVal']
argPathDirection = passed_in_arguments['pathDirection']
# argboundaryVertices = np.array([[37.886928977295476, -76.81533336639404], [37.890527558618025, -76.81533336639404], [37.890527558618025, -76.80932521820068], [37.886928977295476, -76.80932521820068], [37.886928977295476, -76.81533336639404]])
# arghotSpots = np.array([[37.88865631827983, -76.8123185634613]])
# Find bottom left point of boundary:
latLonBoundingBox = minimum_bounding_box(argboundaryVertices)
originPoint = Location(latLonBoundingBox[0], latLonBoundingBox[2])
# Find X/Y distance from bottom left point to all other points:
xyBoundary = []
for point in argboundaryVertices:
tmpPoint = Location(point[0], point[1])
xComp, yComp = originPoint.getXYFromPoint(tmpPoint)
xyBoundary.append([xComp, yComp])
xyBoundaryArray = np.asarray(xyBoundary)
xyHotSpots = []
for point in arghotSpots:
tmpPoint = Location(point[0], point[1])
xComp, yComp = originPoint.getXYFromPoint(tmpPoint)
xyHotSpots.append([xComp, yComp])
xyHotSpotsArray = np.asarray(xyHotSpots)
# X/Y distance to each point is the X/Y coordinates of each point now.
# Create a path to validate at the end:
# originalBoundaryPath = path.Path(xyBoundaryArray)
# Normalize based on the transformed points:
temp_bounding_box = minimum_bounding_box(xyBoundaryArray)
normFactor_x = np.float64(
temp_bounding_box[1] - temp_bounding_box[0]
) # this should always be positive based on the math
normFactor_y = np.float64(temp_bounding_box[3] - temp_bounding_box[2])
normedBoundary = []
for point in xyBoundaryArray:
bnormX = np.float64((point[0] - temp_bounding_box[0]) / normFactor_x)
bnormY = np.float64((point[1] - temp_bounding_box[2]) / normFactor_y)
normedBoundary.append([bnormX, bnormY])
boundaryVerticesNormalized = np.array(normedBoundary)
normedHotSpots = []
for hotPoints in xyHotSpotsArray:
normX = np.float64(
(hotPoints[0] - temp_bounding_box[0]) / normFactor_x)
normY = np.float64(
(hotPoints[1] - temp_bounding_box[2]) / normFactor_y)
normedHotSpots.append([normX, normY])
hotSpotsNormalized = np.array(normedHotSpots)
# END NORMALIZE
# Plan paths:
# **** Start main script **** #
validatePointsPath = path.Path(boundaryVerticesNormalized)
for i in range(len(hotSpotsNormalized)):
if validatePointsPath.contains_point(hotSpotsNormalized[i]) is False:
raise AttributeError(
"One of the selected points of interest is either on or not contained in the boundary: "
+ str(hotSpotsNormalized[i]))
bounding_box = minimum_bounding_box(boundaryVerticesNormalized)
vor = voronoi(hotSpotsNormalized, bounding_box)
#
# fig = pl.figure()
# ax = fig.gca()
# # Plot initial points
# ax.plot(vor.filtered_points[:, 0], vor.filtered_points[:, 1], 'go', markerSize=10)
# Plot ridges points
planningRegions = []
planningHotSpots = []
for region in vor.filtered_regions:
vertices = vor.vertices[region, :]
# ax.plot(vertices[:, 0], vertices[:, 1], 'bo')
planningRegions.append(vertices)
regionPath = path.Path(vertices)
for point in hotSpotsNormalized:
if regionPath.contains_point((point[0], point[1])):
planningHotSpots.append(point)
# Plot ridges
for region in vor.filtered_regions:
vertices = vor.vertices[region + [region[0]], :]
# ax.plot(vertices[:, 0], vertices[:, 1], 'k-')
# Compute and plot centroids
centroids = []
for region in vor.filtered_regions:
vertices = vor.vertices[region + [region[0]], :]
centroid = centroid_region(vertices)
centroids.append(list(centroid[0, :]))
# ax.plot(centroid[:, 0], centroid[:, 1], 'c.')
# Plot boundary supplied:
# ax.plot(xyBoundaryArray[:, 0], xyBoundaryArray[:, 1], 'b-')
# ax.set_xlim([bounding_box[0] - 1, bounding_box[1] + 1])
# ax.set_ylim([bounding_box[2] - 1, bounding_box[3] + 1])
# ax.set_xlim([temp_bounding_box[0] - 1, temp_bounding_box[1] + 1])
# ax.set_ylim([temp_bounding_box[2] - 1, temp_bounding_box[3] + 1])
# Plan the path based on the boundaries:
vehiclePaths = []
vehiclePathsNormalized = []
vehiclePathsLatLon = []
planningRegionsUnNormalized = []
# pVal = 0.01
# gridSpacing = 1.01
# pathDirection = "EastWest"
pVal = argPVal
gridSpacing = argGridVal
pathDirection = argPathDirection
waypointTrimmer = TrimWaypoints(1.0)
for i in xrange(0, len(planningRegions)):
# print "PLANNING REGIONS"
# print planningRegions[i]
tmpWaypoints = planPath(planningRegions[i], pathDirection, gridSpacing,
planningHotSpots[i], pVal)
waypointsInBoundary = []
for point in tmpWaypoints:
if validatePointsPath.contains_point(point):
waypointsInBoundary.append(point)
unNormalizedPoints = []
transformedPoints = []
for point in waypointsInBoundary:
# Un-normalize first:
newX = point[0] * normFactor_x + temp_bounding_box[0]
newY = point[1] * normFactor_y + temp_bounding_box[2]
unNormalizedPoints.append([newY, newX])
# Transform to lat/lon:
distance, bearing = originPoint.getDistanceBearingFromXY(
[newX, newY])
lat, lon = originPoint.getLatLonFromDistanceBearing(
distance, bearing)
transformedPoints.append([lat, lon])
tmpRegion = []
for boundaryPoint in planningRegions[i]:
regionX = boundaryPoint[0] * normFactor_x + temp_bounding_box[0]
regionY = boundaryPoint[1] * normFactor_y + temp_bounding_box[2]
# Transform to lat/lon:
distance, bearing = originPoint.getDistanceBearingFromXY(
[regionX, regionY])
lat, lon = originPoint.getLatLonFromDistanceBearing(
distance, bearing)
tmpRegion.append([lat, lon])
# vehiclePaths.append(unNormalizedPoints)
# vehiclePathsNormalized.append(waypointsInBoundary)
# Trim the waypoint arrays for easier handling:
vehiclePathsTrimmed = waypointTrimmer.trimWPArray(transformedPoints)
vehiclePathsLatLon.append(vehiclePathsTrimmed)
# vehiclePathsLatLon.append(transformedPoints)
planningRegionsUnNormalized.append(tmpRegion)
return planningRegionsUnNormalized, vehiclePathsLatLon
ax.set_xlim([bounding_box[0] - 1, bounding_box[1] + 1])
ax.set_ylim([bounding_box[2] - 1, bounding_box[3] + 1])
# pl.savefig("bounded_voronoi.png")
# sp.spatial.voronoi_plot_2d(vor)
# pl.savefig("voronoi.png")
# Plan the path based on the boundaries:
vehiclePaths = []
vehiclePathsNormalized = []
pVal = 0.5
gridSpacing = 0.25
for i in xrange(0, len(planningRegions)):
# print i
# for region in planningRegions:
tmpWaypoints = planPath(planningRegions[i], "NorthSouth", gridSpacing,
planningHotSpots[i], pVal)
transformedPoints = []
for point in tmpWaypoints:
# Un-normalize first:
newX = point[0] * normFactor_x + temp_bounding_box[0]
newY = point[1] * normFactor_y + temp_bounding_box[2]
# Transform to lat/lon:
#lat,lon = mercToLatLon(newX, newY)plot
#transformedPoints.append([lat, lon])
transformedPoints.append([newY, newX])
vehiclePaths.append(transformedPoints)
vehiclePathsNormalized.append(tmpWaypoints)
# vehiclePaths.append(tmpWaypoints)
# print vehiclePaths # THIS IS WHAT SHOULD BE PLOTTED ON LEAFLET MAP
fc = {"thrust":100,
"mass":4,
"density":1.225,
"cd":1,
"refarea":.25,
"veff": 14}#Flight characteristics of the drone
# STATIC OBSTACLE PRINTING
paths = [] # append your paths to this list to draw them all at once (for comparison)
realpath, allpaths, nonintersectingpath = tanDjikstra.tangentDjikstra(waypoint_list,obstacle_list,bound_segments)
#paths.append(allpaths)
#paths.append(nonintersectingpath)
paths.append(realpath)
#paths.append(retiredalgorithms.rubberpath.rubberpath(obstacle_list,waypoint_list,False,config.FIELD_HEIGHT,config.FIELD_WIDTH))
#paths.append(retiredalgorithms.fanpath.simplefan(obstacle_list,waypoint_list[0],waypoint_list[1],config.FIELD_WIDTH,config.FIELD_HEIGHT))
#paths.append(gravity.simplegravity(obstacle_list, waypoint_list, config.FIELD_HEIGHT, config.FIELD_WIDTH))
save_files.save_files(paths, waypoint_list, obstacle_list)
# load_files.load_files()
drawObstaclesPath(obstacle_list, paths, bound_segments, waypoint_list, config.FIELD_HEIGHT, config.FIELD_WIDTH,config.FILENAME_MAPIMAGE)
# ANIMATION/MOVING OBSTACLE PRINTING
posTimes = planPath.planPath('tandji',obstacle_list,waypoint_list[1:],bound_segments,waypoint_list[0],fc,config)
#Save the positions and the times to a csv file
np.savetxt('AerialPath.csv',posTimes,delimiter=',')
# Plan the path based on the boundaries:
vehiclePaths = []
vehiclePathsNormalized = []
vehiclePathsLatLon = []
planningRegionsUnNormalized = []
# pVal = 0.01
# gridSpacing = 1.01
# pathDirection = "EastWest"
pVal = argPVal
gridSpacing = argGridVal
pathDirection = argPathDirection
for i in xrange(0, len(planningRegions)):
# print "PLANNING REGIONS"
# print planningRegions[i]
tmpWaypoints = planPath(planningRegions[i], pathDirection, gridSpacing, planningHotSpots[i], pVal)
waypointsInBoundary = []
for point in tmpWaypoints:
if validatePointsPath.contains_point(point) == True:
waypointsInBoundary.append(point)
unNormalizedPoints = []
transformedPoints = []
for point in waypointsInBoundary:
# Un-normalize first:
newX = point[0]*normFactor_x + temp_bounding_box[0]
newY = point[1]*normFactor_y + temp_bounding_box[2]
unNormalizedPoints.append([newY, newX])
# Transform to lat/lon: