def findBestLocation(lidardists, location, testingrange, angoffsetrange):
difvsret = {}
# xs = tuple(range(location[0] - testingrange, location[0] + (testingrange + 1)))
# ys = tuple(range(location[1] - testingrange, location[1] + (testingrange + 1)))
# angs = tuple(range(location[2] - angoffsetrange, location[2] + angoffsetrange + 1))
cordrange = np.array(
[[location[0] - testingrange, location[0] + testingrange],
[location[1] - testingrange, location[1] + testingrange]])
cordrange[cordrange <= 0] = 1
cordx = cordrange[0]
cordy = cordrange[1]
cordx[cordx >= size[0]] = size[0] - 1
cordy[cordy >= size[1]] = size[1] - 1
cordrange = np.array([cordx[0], cordx[1], cordy[0], cordy[1]])
xs = tuple(range(cordrange[0], cordrange[1] + 1))
ys = tuple(range(cordrange[2], cordrange[3] + 1))
angs = tuple(
range(location[2] - angoffsetrange, location[2] + angoffsetrange + 1))
fieldlines = CythonLidarPost.openEnvFile("FRC_Field_2018.map")
for x in xs:
for y in ys:
postdata = CythonLidarPost.customRayIntersects(
CythonLidarPost.Point(x, y), 0, fieldlines)
for angle in angs:
idealdists = shiftInds(postdata, angle)
difvsret[findDotDif(lidardists, idealdists)] = [(x, y), angle]
bestdotdif = difvsret[sorted(difvsret)[0]]
reallocation = [bestdotdif[0]]
ang = bestdotdif[1]
return (reallocation, ang), difvsret
def main():
realpoints = [50, 48]
testval = shiftInds(
CythonLidarPost.angledRayIntersects(
CythonLidarPost.Point(realpoints[0], realpoints[1]), 0,
CythonLidarPost.openEnvFile("FRC_Field_2018.map")),
7,
)
printinfo, pointvals = findBestLocation(testval, [49, 49, 2], 5, 15)
# print(printinfo)
dotSvg(findLargestPointPerAngle(pointvals), realpoints)
def speedThisProgramUpDramatically():
postpoints = {}
xs = range(1, 324)
ys = range(1, 648)
angs = range(360)
for x in xs:
for y in ys:
for ang in angs:
postpoints[(x, y, ang)] = CythonLidarPost.angledRayIntersects(
CythonLidarPost.Point(x, y), ang)
json.dump(postpoints, "postpoints.json")
def catchMyDrift(lidarid, location):
# lidardists = parseLidar("COM3")
fieldlines = CythonLidarPost.openEnvFile("FRC_Field_2018.map")
lidardists = CythonLidarPost.customRayIntersects(
CythonLidarPost.Point(location.x, location.y), list(range(1, 50)),
fieldlines)
location, ang, distdifs = findRobotLocation(lidardists, location, 15, 5,
15, fieldlines)
obstacles = measureObstacles(distdifs, 6)
obstaclepoints = extractPointsFromObstacles(obstacles, location,
lidardists)
return location, ang, obstaclepoints
def extractPointsFromObstacles(obstacles, location, lidardists):
points = []
for obstacle in obstacles:
points.append([])
for ang in obstacle:
points[-1].append(
CythonLidarPost.pointFromDistAng(location, ang,
lidardists[ang]))
return points
def findClosestDot(lidardists, minv, maxv, mode, location, ang=None):
variance = np.array(list(range(minv, maxv + 1)))
difvsret = {}
if mode == "location":
xs = variance.copy() + location[0]
ys = variance.copy() + location[1]
for x in xs:
for y in ys:
testdists = CythonLidarPost.angledRayIntersects(
CythonLidarPost.Point(x, y), ang)
difvsret[findDotDif(lidardists, testdists)] = (
x, y) # findDotDif finds the dot sum of the arguments
if mode == "angle":
for var in variance:
testdists = CythonLidarPost.angledRayIntersects(
CythonLidarPost.Point(location[0], location[1]), var)
difvsret[findDotDif(lidardists, testdists)] = var
return difvsret[sorted(difvsret)[-1]]
def findRobotLocation(lidardists, location, angle, testingrange,
angoffsetrange, fieldlines):
difvsret = {}
cordrange = np.array(
[[location.x - testingrange, location.x + testingrange],
[location.y - testingrange, location.y + testingrange]])
cordrange[cordrange <= 0] = 1
cordx = cordrange[0]
cordy = cordrange[1]
cordx[cordx >= size[0]] = size[0] - 1
cordy[cordy >= size[1]] = size[1] - 1
cordrange = np.array([cordx[0], cordx[1], cordy[0], cordy[1]])
xs = tuple(range(int(cordrange[0]), int(cordrange[1] + 1)))
ys = tuple(range(int(cordrange[2]), int(cordrange[3] + 1)))
angs = tuple(range(angle - angoffsetrange, angle + angoffsetrange + 1))
# lidardists = CmToInchesInDict(lidardists)
lidarkeys = list(lidardists.keys())
lidarlist = np.array(list(lidardists.values()))
for x in xs:
for y in ys:
postdata = np.array(
list(
CythonLidarPost.customRayIntersects(
CythonLidarPost.Point(x, y), lidarkeys,
fieldlines).values()))
for angle in angs:
idealdists = shiftInds(postdata, angle)
difvsret[findDotDif(lidarlist, idealdists)] = [
CythonLidarPost.Point(x, y), angle, idealdists
]
bestdotdif = difvsret[sorted(difvsret)[0]]
location = bestdotdif[0]
print(location.x, location.y)
angle = bestdotdif[1]
distdifs = (findDistDifs(lidarlist, bestdotdif[2]))
return location, angle, distdifs
ys = range(1, 648)
angs = range(360)
for x in xs:
for y in ys:
for ang in angs:
postpoints[(x, y, ang)] = CythonLidarPost.angledRayIntersects(
CythonLidarPost.Point(x, y), ang)
json.dump(postpoints, "postpoints.json")
def shiftInds(itershift, offset):
return np.concatenate((itershift[offset:], itershift[:offset]))
# return itershift[offset:] + itershift[:offset]
x = catchMyDrift(0, CythonLidarPost.Point(150, 150))
print(x)
def measureTime(func):
start = time.time()
func()
fin = time.time()
finaltime = fin - start
print("Time: {0}".format(finaltime))
def main():
realpoints = [50, 48]
testval = shiftInds(
CythonLidarPost.angledRayIntersects(