def remap(nmbr, verbose=True, display=True):
"""
Load saved datafile by file 'nmbr'
if verbose: pretty print data
if display: display interactive plot
"""
filename = f'Data/scan_data{nmbr}.pkl'
with open(filename, 'rb') as file:
data = pickle.load(file)
if verbose:
pprint.pprint(data)
pscan = proscan.ProcessScan(data, gap=5, fit=2)
pscan.map(seq_nmbr=nmbr, display_all_points=True, show=display)
def scan_and_plan(nmbr=None):
"""Scan, save data and analyze. Return course & distance to open sector.
"""
if nmbr is None:
nmbr = ''
data = save_scan(nmbr)
pscan = proscan.ProcessScan(data)
logger.debug(f"Regions = {pscan.regions}")
logger.debug(f"Zero Regions = {pscan.zero_regions}")
# find indexes of non-zero regions sorted by number of points
long_regs = pscan.regions_by_length()
for idx in pscan.zero_regions:
long_regs.remove(idx)
# find just left region and right region
long2regs = long_regs[:2]
long2regs.sort()
try:
left_region, right_region = long2regs
except ValueError:
pscan.map(seq_nmbr=nmbr, display_all_points=True)
return 0, 0
# find 'far' end of L & R regions as it will be the constriction
left_pnt_indx = pscan.regions[left_region][-1]
right_pnt_indx = pscan.regions[right_region][0]
# find coords of each point
left_pnt = pscan.points[left_pnt_indx].xy
right_pnt = pscan.points[right_pnt_indx].xy
logger.debug(f"Left point: {left_pnt}")
logger.debug(f"Right point: {right_pnt}")
# construct waypoint halfway between left and right points
waypnt = geo.midpoint(left_pnt, right_pnt)
# Path to travel from origin to waypoint (in center of gap)
r = int(geo.p2p_dist((0, 0), waypnt))
theta = int(geo.p2p_angle((0, 0), waypnt)) # w/r/t +X direction
course = theta - 90 # relative to +Y direction
# print results and display map
logger.debug(f"Relative course: {course}")
logger.debug(f"Travel Distance: {r}")
pscan.map(seq_nmbr=nmbr, display_all_points=True)
return course, r
def get_closest_line_params(nmbr, mapping=False):
"""
Scan and return parameters of most salient line in closest region.
Save scan data in file labeled with nmbr.
Set mapping=True to save plot.
return line parameters as tuple (ccords, length, angle, dist)
"""
data = save_scan(nmbr=nmbr)
pscan = proscan.ProcessScan(data, gap=5, fit=2)
closest_region_idx = pscan.closest_region()
longest_segment = pscan.segments_in_region(closest_region_idx)[0]
line_params = pscan.get_line_parameters(longest_segment)
if mapping:
pscan.map(seq_nmbr=nmbr, display_all_points=True)
return line_params
def polar_coords():
"""
Get polar coords of all the points in scan.
"""
nmbr = input("Enter 'all' or integer number of data to load: ")
if nmbr.isnumeric():
filename = f'Data/scan_data{nmbr}.pkl'
with open(filename, 'rb') as file:
data = pickle.load(file)
pscan = proscan.ProcessScan(data)
#for point in pscan.points:
#print(f"{point.get('theta')*180/math.pi}, {point.get('dist')}")
rvals = [
point.get('dist') for point in pscan.points
if point.get('dist') != 3
]
print(sum(rvals) / len(rvals))
def drive_to_spot(spd=None):
"""
Scan & display interactive map with proposed target spot shown.
User then closes map and either enters 'y' to agree to proposed
spot or 'c' to input coordinates of an alternate one.
Car drives to spot. Repeat.
"""
if not spd:
spd = CARSPEED
nmbr = 100
while True:
# scan & display plot
data = save_scan(nmbr)
pscan = proscan.ProcessScan(data)
pscan.auto_detect_open_sector()
pscan.map(seq_nmbr=nmbr, show=True)
# get coords from user
char = input("Drive to yellow dot? y (c to enter coords): ")
if char in 'yY':
coords = pscan.target
elif char in 'cC':
coordstr = input("Enter x, y coords: ")
if ',' in coordstr:
xstr, ystr = coordstr.split(',')
x = int(xstr)
y = int(ystr)
coords = (x, y)
else:
break
# convert x,y to r, theta then drive
r, theta = geo.r2p(coords)
target_angle = int(theta - 90)
print(f"Turning {target_angle} degrees")
turn_to(car.heading() - target_angle)
print(f"Driving {r:.1f} cm")
drive_ahead(r, spd=spd)
nmbr += 1