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)