def find_true_widths(pts, nvecs, ws, check_scan_location):
tx = pts[:, 0]
ty = pts[:, 1]
onws = ws[:, 0]
opws = ws[:, 1]
stp_sze = 0.1
sf = 0.5 # safety factor
N = len(pts)
nws, pws = [], []
for i in range(N):
pt = [tx[i], ty[i]]
nvec = nvecs[i]
if not check_scan_location(pt):
j = stp_sze
s_pt = lib.add_locations(pt, nvec, j)
while not check_scan_location(s_pt) and j < opws[i]:
j += stp_sze
s_pt = lib.add_locations(pt, nvec, j)
pws.append(j*sf)
j = stp_sze
s_pt = lib.sub_locations(pt, nvec, j)
while not check_scan_location(s_pt) and j < onws[i]:
j += stp_sze
s_pt = lib.sub_locations(pt, nvec, j)
nws.append(j*sf)
# print(f"Pt added without being adjusted")
else:
print(f"Obs in way of pt: {i}")
for j in np.linspace(0, onws[i], 10):
p_pt = lib.add_locations(pt, nvec, j)
n_pt = lib.sub_locations(pt, nvec, j)
if not check_scan_location(p_pt):
nws.append(-j*(1+sf))
pws.append(opws[i])
print(f"PosPt NewW: [{-j*(1+sf)}, {opws[i]}]")
break
elif not check_scan_location(n_pt):
pws.append(-j*(1+sf))
nws.append(onws[i])
print(f"PosPt NewW: [{-j*(1+sf)}, {onws[i]}]")
break
if j == onws[i]:
print(f"Problem - no space found")
nws, pws = np.array(nws), np.array(pws)
ws = np.concatenate([nws[:, None], pws[:, None]], axis=-1)
return ws
def find_centerline(self, show=True):
dt = self.dt
d_search = 0.8
n_search = 11
dth = (np.pi * 4/5) / (n_search-1)
# makes a list of search locations
search_list = []
for i in range(n_search):
th = -np.pi/2 + dth * i
x = -np.sin(th) * d_search
y = np.cos(th) * d_search
loc = [x, y]
search_list.append(loc)
pt = start = np.array([self.conf.sx, self.conf.sy])
self.cline = [pt]
th = self.stheta
while (lib.get_distance(pt, start) > d_search/2 or len(self.cline) < 10) and len(self.cline) < 500:
vals = []
self.search_space = []
for i in range(n_search):
d_loc = lib.transform_coords(search_list[i], -th)
search_loc = lib.add_locations(pt, d_loc)
self.search_space.append(search_loc)
x, y = self.xy_to_row_column(search_loc)
val = dt[y, x]
vals.append(val)
ind = np.argmax(vals)
d_loc = lib.transform_coords(search_list[ind], -th)
pt = lib.add_locations(pt, d_loc)
self.cline.append(pt)
if show:
self.plot_raceline_finding()
th = lib.get_bearing(self.cline[-2], pt)
print(f"Adding pt: {pt}")
self.cline = np.array(self.cline)
self.N = len(self.cline)
print(f"Raceline found --> n: {len(self.cline)}")
if show:
self.plot_raceline_finding(True)
self.plot_raceline_finding(False)
def expand_wpts(self):
n = 5 # number of pts per orig pt
dz = 1 / n
o_line = self.wpts
o_ss = self.ss
o_vs = self.vs
new_line = []
new_ss = []
new_vs = []
for i in range(self.N - 1):
dd = lib.sub_locations(o_line[i + 1], o_line[i])
for j in range(n):
pt = lib.add_locations(o_line[i], dd, dz * j)
new_line.append(pt)
ds = o_ss[i + 1] - o_ss[i]
new_ss.append(o_ss[i] + dz * j * ds)
dv = o_vs[i + 1] - o_vs[i]
new_vs.append(o_vs[i] + dv * j * dz)
self.wpts = np.array(new_line)
self.ss = np.array(new_ss)
self.vs = np.array(new_vs)
self.N = len(new_line)
def expand_wpts(self):
n = 5 # number of pts per orig pt
dz = 1 / n
o_line = self.waypoints[:, 0:2]
# o_ss = self.ss
o_vs = self.waypoints[:, 2]
new_line = []
# new_ss = []
new_vs = []
for i in range(len(self.waypoints) - 1):
dd = lib.sub_locations(o_line[i + 1], o_line[i])
for j in range(n):
pt = lib.add_locations(o_line[i], dd, dz * j)
new_line.append(pt)
# ds = o_ss[i+1] - o_ss[i]
# new_ss.append(o_ss[i] + dz*j*ds)
dv = o_vs[i + 1] - o_vs[i]
new_vs.append(o_vs[i] + dv * j * dz)
wpts = np.array(new_line)
# self.ss = np.array(new_ss)
vs = np.array(new_vs)
self.waypoints = np.concatenate([wpts, vs[:, None]], axis=-1)
