def check_done_reward_track_train(self):
"""
Checks if the race lap is complete
Returns
Done flag
"""
self.reward = 0 # normal
if self.env_map.check_scan_location([self.car.x, self.car.y]):
self.done = True
self.colission = True
self.reward = -1
self.done_reason = f"Crash obstacle: [{self.car.x:.2f}, {self.car.y:.2f}]"
# horizontal_force = self.car.mass * self.car.th_dot * self.car.velocity
# self.y_forces.append(horizontal_force)
# if horizontal_force > self.car.max_friction_force:
# self.done = True
# self.reward = -1
# self.done_reason = f"Friction limit reached: {horizontal_force} > {self.car.max_friction_force}"
if self.steps > self.max_steps:
self.done = True
self.done_reason = f"Max steps"
car = [self.car.x, self.car.y]
cur_end_dis = lib.get_distance(car, self.env_map.start_pose[0:2])
if cur_end_dis < self.end_distance and self.steps > 800:
self.done = True
self.reward = 1
self.done_reason = f"Lap complete, d: {cur_end_dis}"
return self.done
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 add_obstacles(self):
obs_img = np.zeros_like(self.obs_img)
obs_size_m = np.array([self.obs_size, self.obs_size])
obs_size_px = obs_size_m / self.resolution
rands = np.random.uniform(size=(self.n_obs, 2))
idx_rands = rands[:, 0] * len(self.ws)
w_rands = (rands[:, 1] * 2 - np.ones_like(rands[:, 1]))
w_rands = w_rands * np.mean(
self.ws
) * 0.3 # x average length, adjusted to be both sides of track
# magic 0.8 is to keep the obstacles closer to the center of the track
obs_locations = []
for i in range(self.n_obs):
idx = idx_rands[i]
w = w_rands[i]
int_idx = int(idx) # note that int always rounds down
# start with using just int_idx
n = self.nvecs[i]
offset = np.array([n[0] * w, n[1] * w])
location = lib.add_locations(self.t_pts[int_idx], offset)
if lib.get_distance(location, self.start_pose[0:2]) < 1:
continue
# location = np.flip(location)
# location = self.t_pts[int_idx]
rc_location = self.xy_to_row_column(location)
location = np.array(location, dtype=int)
obs_locations.append(rc_location)
obs_locations = np.array(obs_locations)
for location in obs_locations:
x, y = location[0], location[1]
for i in range(0, int(obs_size_px[0])):
for j in range(0, int(obs_size_px[1])):
if x + i < self.map_width and y + j < self.map_height:
# obs_img[x+i, y+j] = 255
obs_img[y + j, x + i] = 255
self.obs_img = obs_img