def test_graph_traj_costing(graph: Graph, planner: LatticeDstarLite,
max_state):
# create obstacle in map
dx = planner.dstate[0]
obst_xi = 20
min_x, _, _, _, _ = planner.key_to_state(
[obst_xi, 0, planner.thetas[0], planner.velocities[0], 0])
graph.map[:, obst_xi:obst_xi + 5] = graph.wheel_radius + \
1 # large jump along x = 5
# facing obstacle at xi = 5
state = [min_x - 10 * dx, 0, 0, planner.get_vel(state=max_state), 0]
action = Action(steer=0, v=planner.get_vel(state=max_state))
ai = planner.actions.index(action)
traj = planner.car.rollout(state=state[:3],
action=action,
dt=planner.dt,
T=planner.T)
costs, valid_traj = graph.calc_cost_and_validate_traj(state,
traj,
action=action,
ai=ai)
# trajectory should have been truncated since collides with obstacle
assert (valid_traj.shape[0] < traj.shape[0])
assert (valid_traj[-1, 0] < 5) # valid traj stops before obstacle
def test_graph_update_map(graph: Graph, min_state, max_state):
dx, dy, _, _, _ = graph.dstate
# empty window should not cause error
xbounds = [0, 0]
ybounds = [0, 0]
graph.update_map(xbounds, ybounds, obs_window=[])
# single-value window at very edge of map should not cause error
xbounds = [0, 1]
ybounds = [0, 1]
graph.update_map(xbounds, ybounds, obs_window=np.zeros((1, 1)))
# near min and max with window extending to edge
# state is diagonal neighbor of map's corner, window size limited to 1
# |
# |
# | x
# |________
width = 5
height = 8
xbounds = [0, width]
ybounds = [0, height]
window = np.ones((height, width))
need_update = graph.update_map(xbounds, ybounds, obs_window=window)
assert (need_update)
assert (np.allclose(graph.map[0:height, 0:width], window))
# reset map to not mess up other tests
graph.map.fill(0)
def get_obs_window_bounds(graph: Graph, state, width, height):
xi, yi, _, _, _ = graph.discretize(state)
half_width = int(width / 2)
half_height = int(height / 2)
xbounds = (max(0, xi - half_width), min(graph.width, xi + half_width))
ybounds = (max(0, yi - half_height), min(graph.height, yi + half_height))
return xbounds, ybounds
def create_planner(cost_weights, thetas, steer_angles, velocities, car,
min_state, dstate, prior_map, eps, T):
# create planner
Astar_graph = Graph(map=prior_map,
min_state=min_state,
dstate=dstate,
thetas=thetas,
wheel_radius=car.wheel_radius,
cost_weights=cost_weights)
astar_planner = LatticeAstar(graph=Astar_graph,
car=car,
min_state=min_state,
dstate=dstate,
velocities=velocities,
steer_angles=steer_angles,
thetas=thetas,
T=T,
eps=eps,
viz=True)
return astar_planner
def run_all_tests():
"""
test upper and lower bounds of half bins
test that out-of-bound assertions are called
"""
# define car
wheel_radius = 24 / 2.0 * INCH_TO_M
# define position space
dy, dx = 0.1, 0.1
miny, minx = -2, -2
maxy, maxx = 8, 8
Y = int(round((maxy - miny + dy) / dy)) # [min, max] inclusive
X = int(round((maxx - minx + dx) / dx))
map = np.zeros((Y, X))
# define action space
velocities = np.linspace(start=1, stop=3, num=3)
dv = velocities[1] - velocities[0]
dt = 0.1
T = 1.0
steer_angles = np.linspace(-math.pi / 4, math.pi / 4, 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)
# NOTE: Actual thetas are in radians! here just shown in degrees for clarity
assert (np.allclose(thetas, [0, 45, 90, 135, 180, 225, 270, 315]))
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])
# arbitrary
start = [0, 0, 0, 0, 0] # time doesn't matter here
goal = [5, 5, 0, 0, 0] # time doesn't matter here
cost_weights = [1, 1, 1]
graph = Graph(map=map,
min_state=min_state,
dstate=dstate,
thetas=thetas,
velocities=velocities,
wheel_radius=wheel_radius,
cost_weights=cost_weights)
planner = LatticeDstarLite(graph=graph,
min_state=min_state,
dstate=dstate,
velocities=velocities,
steer_angles=steer_angles,
thetas=thetas,
T=T)
# irrelevant for time
max_state = np.array([maxx, maxy, thetas[-1], velocities[-1], 0])
test_state_equal(min_state=min_state,
max_state=max_state,
dstate=dstate,
thetas=thetas,
velocities=velocities,
planner=planner,
graph=graph)
test_graph_update_map(graph, min_state, max_state)
test_graph_traj_costing(graph, planner, max_state)
test_visualize_primitives(graph)
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)