def reset(self):
# Destroy all objects
self._destroy()
# TODO: decide on the reward
self.reward = 0.0
self.prev_reward = 0.0
self.tot_reward = [0.0 for _ in range(NUM_VEHICLES)]
# Transition for zoom
self.t = make_n_times(NUM_VEHICLES)
# Rendering values
self.road_poly = []
self.human_render = False
while True:
success = self._create_track()
if success: break
print("retry to generate track (normal if there are not many of this messages)")
# randomly init each car
if not self.init_pos:
rect_poly_indices = [i for i in range(len(self.directions)) if self.directions[i] in "lrtb"]
random_choices = np.random.choice(rect_poly_indices, NUM_VEHICLES, replace=False)
for car_idx, rid in enumerate(random_choices):
rect_poly = np.array(self.road_poly[rid][0])
direction = self.directions[rid]
x = np.mean(rect_poly[:, 0])
y = np.mean(rect_poly[:, 1])
if direction == "r":
angle = -90
elif direction == "t":
angle = 0
elif direction == "l":
angle = 90
else:
angle = 180
self.cars[car_idx] = Car(self.world, angle*math.pi/180.0, x, y)
else:
for car_idx, init_p in enumerate(self.init_pos):
i, j, angle = init_p
i -= 0.5
j += 0.5
x, y = i*EDGE_WIDTH, j*EDGE_WIDTH
x += self.off_params[0]
y += self.off_params[1]
self.cars[car_idx] = Car(self.world, angle*math.pi/180.0, x, y)
# return states after init
return self.step([None for i in range(NUM_VEHICLES)])[0]
def init_planner(prior_map):
# load in vehicle params
with open(CAR_PARAMS_FILE, 'r') as f:
car_params = yaml.load(f)
# define search params
eps = 2.0
dist_cost = 1
time_cost = 1
roughness_cost = 1
cost_weights = (dist_cost, time_cost, roughness_cost)
# Define action space
dt = 0.1
T = 1.0
# velocities
# TODO: Let handle negative velocities, but enforce basic dynamic windowing
max_speed = car_params["max_speed"]
num_speed = 3
velocities = np.linspace(start=0, stop=max_speed, num=num_speed) / dt
dv = velocities[1] - velocities[0]
# steer angles
max_abs_steer = car_params["max_steer"]
num_steer = 3
steer_angles = np.linspace(-max_abs_steer, max_abs_steer, num=num_steer)
# create simple car model for planning
mid_to_wheel_length = car_params["car_length"] / 2.0
car = Car(L=mid_to_wheel_length,
max_v=car_params["max_speed"],
max_steer=car_params["max_steer"],
wheel_radius=car_params["wheel_radius"])
# define heading space
start, stop, step = 0, 315, 45
num_thetas = int((stop - start) / step) + 1
thetas = np.linspace(start=0, stop=315, num=num_thetas)
thetas = thetas / RAD_TO_DEG # convert to radians
dtheta = step / RAD_TO_DEG
# collective variables for discretizing C-sapce
dy, dx = 1.0, 1.0
miny, minx = 0, 0
dstate = np.array([dx, dy, dtheta, dv, dt])
min_state = np.array([minx, miny, min(thetas), min(velocities), 0])
# get planners
planner = create_planner(cost_weights=cost_weights,
thetas=thetas,
steer_angles=steer_angles,
velocities=velocities,
car=car,
min_state=min_state,
dstate=dstate,
prior_map=prior_map,
eps=eps,
T=T)
# store planner in file to be used for planning
with open(PLANNER_FILE, "w") as f:
pickle.dump(planner, f)
def _reset(self):
self._destroy()
self.reward = 0.0
self.prev_reward = 0.0
self.tile_visited_count = 0
self.t = 0.0
self.road_poly = []
self.human_render = False
while True:
success = self._create_track()
if success: break
print("retry to generate track (normal if there are not many of this messages)")
self.car = Car(self.world, *self.track[0][1:4])
return self._step(None)[0]
def reset(self):
self._destroy()
self.reward = 0.0
self.t = 0.0
self.road_poly = []
self.state = State()
while True:
success = self._create_track()
if success: break
print(
"retry to generate track (normal if there are not many of this messages)"
)
self.car = Car(self.world, *self.track[0][1:4])
return self.step(None)[0]
def reset(self, track=None):
self._destroy()
self.reward = 0.0
self.prev_reward = 0.0
self.tile_visited_count = 0
self.t = 0.0
self.road_poly = []
self.track_pack = None
if track is None:
while True:
success = self._create_track([], [])
if success:
break
else:
self._create_track(track['noises'], track['rads'])
self.car = Car(self.world, *self.track[0][1:4])
return self.step(None)[0]
def reset(self):
self._destroy()
self.reward = 0.0
self.prev_reward = 0.0
self.tile_visited_count = 0
self.t = 0.0
self.road_poly = []
while True:
success = self._create_track()
if success:
break
if self.verbose == 1:
print("retry to generate track (normal if there are not many"
"instances of this message)")
self.car = Car(self.world,
*self.track[0][1:4],
sensors_activated=self.sensors_activated)
return self.step([0, 0.1, 0])[0]
def main():
# load in map
map_file = "search_planning_algos/maps/map3.npy"
map = np.load(map_file)
Y, X = map.shape
dy, dx = 1.0, 1.0
miny, minx = 0, 0
# define car
wheel_radius = 0 # anything non-zero is an obstacle
# define search params
eps = 4
dist_cost = 1
time_cost = 1
roughness_cost = 1
cost_weights = (dist_cost, time_cost, roughness_cost)
# define action space
dt = 0.1
T = 1.0
velocities = np.linspace(start=1, stop=2, num=2) / dt
dv = velocities[1] - velocities[0]
steer_angles = np.linspace(-math.pi / 32, math.pi / 32, num=5)
# define heading space
start, stop, step = 0, 315, 45
num_thetas = int((stop - start) / step) + 1
thetas = np.linspace(start=0, stop=315, num=num_thetas)
thetas = thetas / RAD_TO_DEG # convert to radians
dtheta = step / RAD_TO_DEG
# collective variables for discretizing C-sapce
dstate = np.array([dx, dy, dtheta, dv, dt])
min_state = np.array([minx, miny, min(thetas), min(velocities), 0])
# create planner and graph
prior_map = np.zeros_like(map)
graph = Graph(map=prior_map,
min_state=min_state,
dstate=dstate,
thetas=thetas,
wheel_radius=wheel_radius,
cost_weights=cost_weights)
car = Car(max_steer=max(steer_angles), max_v=max(velocities))
planner = LatticeDstarLite(graph=graph,
car=car,
min_state=min_state,
dstate=dstate,
velocities=velocities,
steer_angles=steer_angles,
thetas=thetas,
T=T,
eps=eps,
viz=True)
# define start and goal (x,y) need to be made continuous
# since I selected those points on image map of discrete space
start = (np.array([60, 40, 0, velocities[0], 0]) *
np.array([dx, dy, 1, 1, 1]))
# looks like goal should face up, but theta is chosen
# in image-frame as is the y-coordinates, so -90 faces
# upwards on our screen and +90 faces down... it looks
goal = (np.array([85, 65, -math.pi / 2, velocities[0], 0]) *
np.array([dx, dy, 1, 1, 1]))
# run planner
simulate_plan_execution(start=start,
goal=goal,
planner=planner,
true_map=map)