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)
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 min_render(self, wait=False):
fig = plt.figure(4)
plt.clf()
ret_map = self.env_map.scan_map
plt.imshow(ret_map)
# plt.xlim([0, self.env_map.width])
# plt.ylim([0, self.env_map.height])
s_x, s_y = self.env_map.convert_to_plot(self.env_map.start)
plt.plot(s_x, s_y, '*', markersize=12)
c_x, c_y = self.env_map.convert_to_plot([self.car.x, self.car.y])
plt.plot(c_x, c_y, '+', markersize=16)
for i in range(self.scan_sim.number_of_beams):
angle = i * self.scan_sim.dth + self.car.theta - self.scan_sim.fov / 2
fs = self.scan_sim.scan_output[
i] * self.scan_sim.n_searches * self.scan_sim.step_size
dx = [np.sin(angle) * fs, np.cos(angle) * fs]
range_val = lib.add_locations([self.car.x, self.car.y], dx)
r_x, r_y = self.env_map.convert_to_plot(range_val)
x = [c_x, r_x]
y = [c_y, r_y]
plt.plot(x, y)
for pos in self.action_memory:
p_x, p_y = self.env_map.convert_to_plot(pos)
plt.plot(p_x, p_y, 'x', markersize=6)
text_start = self.env_map.width + 10
spacing = int(self.env_map.height / 10)
s = f"Reward: [{self.reward:.1f}]"
plt.text(text_start, spacing * 1, s)
s = f"Action: [{self.action[0]:.2f}, {self.action[1]:.2f}]"
plt.text(text_start, spacing * 2, s)
s = f"Done: {self.done}"
plt.text(text_start, spacing * 3, s)
s = f"Pos: [{self.car.x:.2f}, {self.car.y:.2f}]"
plt.text(text_start, spacing * 4, s)
s = f"Vel: [{self.car.velocity:.2f}]"
plt.text(text_start, spacing * 5, s)
s = f"Theta: [{(self.car.theta * 180 / np.pi):.2f}]"
plt.text(text_start, spacing * 6, s)
s = f"Delta x100: [{(self.car.steering*100):.2f}]"
plt.text(text_start, spacing * 7, s)
s = f"Theta Dot: [{(self.car.th_dot):.2f}]"
plt.text(text_start, spacing * 8, s)
s = f"Steps: {self.steps}"
plt.text(100, spacing * 9, s)
plt.pause(0.0001)
if wait:
plt.show()
def trace_ray(self, x, y, theta, noise=True):
# obs_res = 10
for j in range(self.n_searches): # number of search points
fs = self.step_size * (j + 1
) # search from 1 step away from the point
dx = [np.sin(theta) * fs, np.cos(theta) * fs]
search_val = lib.add_locations([x, y], dx)
if self._check_location(search_val):
break
ray = (
j) / self.n_searches #* (1 + np.random.normal(0, self.std_noise))
return ray
def render(self,
wait=False,
scan_sim=None,
save=False,
pts1=None,
pts2=None):
self.env_map.render_map(4)
fig = plt.figure(4)
if scan_sim is not None:
for i in range(scan_sim.number_of_beams):
angle = i * scan_sim.dth + self.car.theta - scan_sim.fov / 2
fs = scan_sim.scan_output[
i] * scan_sim.n_searches * scan_sim.step_size
dx = [np.sin(angle) * fs, np.cos(angle) * fs]
range_val = lib.add_locations([self.car.x, self.car.y], dx)
cx, cy = self.env_map.convert_position(
[self.car.x, self.car.y])
rx, ry = self.env_map.convert_position(range_val)
x = [cx, rx]
y = [cy, ry]
plt.plot(x, y)
xs, ys = [], []
for pos in self.action_memory:
x, y = self.env_map.xy_to_row_column(pos)
xs.append(x)
ys.append(y)
plt.plot(xs, ys, 'r', linewidth=3)
plt.plot(xs, ys, '+', markersize=12)
x, y = self.env_map.xy_to_row_column([self.car.x, self.car.y])
plt.plot(x, y, 'x', markersize=20)
if pts1 is not None:
for i, pt in enumerate(pts1):
x, y = self.env_map.convert_position(pt)
plt.text(x, y, f"{i}")
plt.plot(x, y, 'x', markersize=10)
if pts2 is not None:
for pt in pts2:
x, y = self.env_map.convert_position(pt)
plt.plot(x, y, 'o', markersize=6)
text_x = self.env_map.obs_img.shape[0] + 10
text_y = self.env_map.obs_img.shape[1] / 10
s = f"Reward: [{self.reward:.1f}]"
plt.text(text_x, text_y * 1, s)
s = f"Action: [{self.action[0]:.2f}, {self.action[1]:.2f}]"
plt.text(text_x, text_y * 2, s)
s = f"Done: {self.done}"
plt.text(text_x, text_y * 3, s)
s = f"Pos: [{self.car.x:.2f}, {self.car.y:.2f}]"
plt.text(text_x, text_y * 4, s)
s = f"Vel: [{self.car.velocity:.2f}]"
plt.text(text_x, text_y * 5, s)
s = f"Theta: [{(self.car.theta * 180 / np.pi):.2f}]"
plt.text(text_x, text_y * 6, s)
s = f"Delta x100: [{(self.car.steering*100):.2f}]"
plt.text(text_x, text_y * 7, s)
s = f"Done reason: {self.done_reason}"
plt.text(text_x, text_y * 8, s)
s = f"Steps: {self.steps}"
plt.text(text_x, text_y * 9, s)
plt.pause(0.0001)
if wait:
plt.show()
if save and self.eps % 2 == 0:
plt.savefig(f'TrainingFigs/t{self.eps}.png')