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')