def load_center_pts(self):
track_data = []
filename = 'maps/' + self.map_name + '_std.csv'
try:
with open(filename, 'r') as csvfile:
csvFile = csv.reader(csvfile, quoting=csv.QUOTE_NONNUMERIC)
for lines in csvFile:
track_data.append(lines)
except FileNotFoundError:
raise FileNotFoundError("No map file center pts")
track = np.array(track_data)
print(f"Track Loaded: {filename} in reward")
N = len(track)
self.wpts = track[:, 0:2]
ss = np.array([
lib.get_distance(self.wpts[i], self.wpts[i + 1])
for i in range(N - 1)
])
ss = np.cumsum(ss)
self.ss = np.insert(ss, 0, 0)
self.total_s = self.ss[-1]
self.diffs = self.wpts[1:, :] - self.wpts[:-1, :]
self.l2s = self.diffs[:, 0]**2 + self.diffs[:, 1]**2
def convert_pts_s_th(pts):
N = len(pts)
s_i = np.zeros(N-1)
th_i = np.zeros(N-1)
for i in range(N-1):
s_i[i] = lib.get_distance(pts[i], pts[i+1])
th_i[i] = lib.get_bearing(pts[i], pts[i+1])
return s_i, th_i
def init_agent(self, env_map):
self.env_map = env_map
self.scan_sim.set_check_fcn(self.env_map.check_scan_location)
# self.wpts = self.env_map.get_min_curve_path()
self.wpts = self.env_map.get_reference_path()
self.prev_dist_target = lib.get_distance(self.env_map.start,
self.env_map.end)
return self.wpts
def cth_reward(self, s_p):
pt_i, pt_ii, d_i, d_ii = find_closest_pt(s_p[0:2], self.wpts)
d = lib.get_distance(pt_i, pt_ii)
d_c = get_tiangle_h(d_i, d_ii, d) / self.dis_scale
th_ref = lib.get_bearing(pt_i, pt_ii)
th = s_p[2]
d_th = abs(lib.sub_angles_complex(th_ref, th))
v_scale = s_p[3] / self.max_v
r = self.mh * np.cos(d_th) * v_scale - self.md * d_c
return r
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 __call__(self, s, a, s_p, r, dev):
if r == -1:
return r
else:
pt_i, pt_ii, d_i, d_ii = find_closest_pt(s_p[0:2], self.wpts)
d = lib.get_distance(pt_i, pt_ii)
d_c = get_tiangle_h(d_i, d_ii, d) / self.dis_scale
th_ref = lib.get_bearing(pt_i, pt_ii)
th = s_p[2]
d_th = abs(lib.sub_angles_complex(th_ref, th))
v_scale = s_p[3] / self.max_v
new_r = self.mh * np.cos(d_th) * v_scale - self.md * d_c
return new_r + r
def find_closest_pt(pt, wpts):
"""
Returns the two closes points in order along wpts
"""
dists = [lib.get_distance(pt, wpt) for wpt in wpts]
min_i = np.argmin(dists)
d_i = dists[min_i]
if min_i == len(dists) - 1:
min_i -= 1
if dists[max(min_i - 1, 0)] > dists[min_i + 1]:
p_i = wpts[min_i]
p_ii = wpts[min_i + 1]
d_i = dists[min_i]
d_ii = dists[min_i + 1]
else:
p_i = wpts[min_i - 1]
p_ii = wpts[min_i]
d_i = dists[min_i - 1]
d_ii = dists[min_i]
return p_i, p_ii, d_i, d_ii
def distance_potential(s, s_p, end, beta=0.2, scale=0.5):
prev_dist = lib.get_distance(s[0:2], end)
cur_dist = lib.get_distance(s_p[0:2], end)
d_dis = (prev_dist - cur_dist) / scale
return d_dis * beta
