def test_world_size_and_center():
costmap = CostMap2D.create_empty((1, 1), 0.5, world_origin=(0, 0))
assert costmap.get_data().shape == (2, 2)
np.testing.assert_array_equal(costmap.world_bounds(), [0, 1, 0, 1])
np.testing.assert_array_equal(costmap.world_size(), [1, 1])
np.testing.assert_array_equal(costmap.world_center(), [0.5, 0.5])
costmap = CostMap2D.create_empty((1, 1), 0.5, world_origin=(-0.13, 0.23))
assert costmap.get_data().shape == (2, 2)
np.testing.assert_array_equal(costmap.world_bounds(), [-0.13, 1-0.13, 0.23, 1+0.23])
np.testing.assert_array_equal(costmap.world_size(), [1, 1])
np.testing.assert_array_equal(costmap.world_center(), [0.5-0.13, 0.5+0.23])
costmap = CostMap2D.create_empty((1, 1), 0.05, world_origin=(-0.13, 0.23))
assert costmap.get_data().shape == (20, 20)
np.testing.assert_array_equal(costmap.world_bounds(), [-0.13, 1-0.13, 0.23, 1+0.23])
np.testing.assert_array_equal(costmap.world_size(), [1, 1])
np.testing.assert_array_equal(costmap.world_center(), [0.5-0.13, 0.5+0.23])
costmap = CostMap2D.create_empty((1, 1), 0.03, world_origin=(-0.13, 0.23))
assert costmap.get_data().shape == (33, 33)
# if there is not an exact match with resolution, we create smaller map than requested
np.testing.assert_array_equal(costmap.world_bounds(), [-0.13, 1-0.13-0.01, 0.23, 1+0.23-0.01])
np.testing.assert_array_equal(costmap.world_size(), [1-0.01, 1-0.01])
np.testing.assert_array_equal(costmap.world_center(), [0.5-0.13-0.005, 0.5+0.23-0.005])
def test_draw_robot_full():
costmap = CostMap2D.create_empty((5, 5), 0.05)
array_to_draw = np.zeros((costmap.get_data().shape + (3, )),
dtype=np.uint8)
draw_robot(array_to_draw,
_tricycle_footprint(), (2, 2, np.pi / 4),
costmap.get_resolution(),
costmap.get_origin(),
color=(0, 0, 150))
draw_robot(array_to_draw,
_tricycle_footprint(), (-0.5, -0.5, np.pi / 4),
costmap.get_resolution(),
costmap.get_origin(),
color=(0, 0, 150))
draw_robot(array_to_draw,
_tricycle_footprint(), (4.6, 3, np.pi / 4),
costmap.get_resolution(),
costmap.get_origin(),
color=(0, 0, 150))
draw_robot(array_to_draw,
_tricycle_footprint(), (-4234.6, 3456, np.pi / 4),
costmap.get_resolution(),
costmap.get_origin(),
color=(0, 0, 150))
static_map = CostMap2D.create_empty((5, 5), 0.05)
img = prepare_canvas(static_map.get_data().shape)
draw_robot(img, _tricycle_footprint(), [0., -3, np.pi / 2.],
static_map.get_resolution(), static_map.get_origin())
def prepare_map_and_path(params):
"""
Prepare costmap and static path based on the mini env params
:param params MiniEnvParams: params of the mini env
:return Tuple[CostMap2D, np.ndarray(3, 2)]: costmap with obstacles and path to follow
"""
obstacles = [
Wall(from_pt=params.obstacle_o.as_np(),
to_pt=params.obstacle_a.as_np()),
Wall(from_pt=params.obstacle_o.as_np(),
to_pt=params.obstacle_b.as_np()),
]
obstacle = Block(poly_pt=np.asarray([
params.obstacle_o.as_np(),
params.obstacle_a.as_np(),
params.obstacle_b.as_np()
]))
coarse_static_path = np.array(
[params.start_pos.as_np(),
params.end_pos.as_np()])
static_map = CostMap2D.create_empty(
world_size=(params.h, params.w), # x width, y height
resolution=params.env_params.resolution,
world_origin=(-params.h / 2., -params.w / 2.))
if not params.turn_off_obstacles:
# for obs in obstacles:
# static_map = obs.render(static_map)
obstacle.render(static_map)
return static_map, coarse_static_path
def box_3d_planning(debug):
costmap = CostMap2D.create_empty((4, 4), 0.05, np.zeros((2,)))
gap = 1.4
add_wall_to_static_map(costmap, (0, 2), (2, 2), width=0.5)
add_wall_to_static_map(costmap, (2+gap, 2), (4, 2), width=0.5)
footprint_width = 0.79
footprint = np.array(
[[0.5*footprint_width, 0.5*footprint_width],
[-0.5 * footprint_width+1e-6, 0.5 * footprint_width],
[-0.5 * footprint_width+1e-6, -0.5 * footprint_width+1e-6],
[0.5 * footprint_width, -0.5 * footprint_width+1e-6]]
)
motion_primitives = exhaustive_geometric_primitives(
costmap.get_resolution(), 10, 32
)
start_pose = np.array([2.3, 1.0, np.pi/4])
goal_pose = np.array([2.6, 3.0, np.pi/4])
plan_xytheta, plan_xytheta_cell, controls, plan_time, solution_eps, environment = perform_single_planning(
planner_name='arastar',
footprint=footprint,
motion_primitives=motion_primitives,
forward_search=True,
costmap=costmap,
start_pose=start_pose,
goal_pose=goal_pose,
target_v=0.65,
target_w=1.0,
allocated_time=np.inf,
cost_scaling_factor=40.,
debug=False)
if debug:
print(environment.xytheta_real_to_cell(start_pose))
print(environment.xytheta_real_to_cell(goal_pose))
print(plan_xytheta_cell)
print("done with the solution of size=%d and sol. eps=%f" % (len(plan_xytheta_cell), solution_eps))
print("actual path (with intermediate poses) size=%d" % len(plan_xytheta))
params = environment.get_params()
costmap = environment.get_costmap()
img = prepare_canvas(costmap.shape)
draw_world_map_with_inflation(img, costmap)
for pose in plan_xytheta:
draw_robot(img, footprint, pose, params.cellsize_m, np.zeros((2,)),
color=70, color_axis=(1, 2))
draw_trajectory(img, params.cellsize_m, np.zeros((2,)), plan_xytheta)
draw_robot(img, footprint, start_pose, params.cellsize_m, np.zeros((2,)))
draw_robot(img, footprint, goal_pose, params.cellsize_m, np.zeros((2,)))
magnify = 4
img = cv2.resize(img, dsize=(0, 0), fx=magnify, fy=magnify, interpolation=cv2.INTER_NEAREST)
cv2.imshow("planning result", img)
cv2.waitKey(-1)
def load_costmap_from_img(img_fname):
"""
Make a costmap from an image.
:param img_fname str: fname of image to load.
:return (path_to_follow, CostMap2D) pair: path following task specification
"""
resolution = 0.03
world_size = 10
static_map = CostMap2D.create_empty(world_size=(world_size, world_size),
resolution=resolution,
world_origin=(0, 0))
img = cv2.imread(img_fname)
# opencv loads bgr
b, g, _ = img[..., 0], img[..., 1], img[..., 2]
start_img = g
# walls_img = r
path_img = b
start_y, start_x = np.where(start_img == 255)
start_x = start_x[0]
start_y = start_y[0]
current_node = start_x, start_y
path = []
found_neighbour = True
while found_neighbour:
path.append(np.array(current_node))
cur_x, cur_y = current_node
found_neighbour = False
for v_x, v_y in _neighbours():
n_x = cur_x + v_x
n_y = cur_y + v_y
val = path_img[n_y, n_x]
if val == 255:
current_node = n_x, n_y
found_neighbour = True
path_img[n_y, n_x] = 0
break
prepath = np.array(path)
path = prepath * resolution
# Should actually do this, but not allowed for now
# static_map._data = np.clip(walls_img, 0, 254)
path = orient_path(path)
return path, static_map
def test_rotate_costmap():
# test inflated map
costmap = CostMap2D.create_empty((5, 4), 0.05)
costmap.get_data()[30, 30] = CostMap2D.LETHAL_OBSTACLE
assert_mark_at(costmap, 30, 30)
rotated_costmap = CostMap2D(rotate_costmap(costmap.get_data(), -np.pi / 4),
costmap.get_resolution(), costmap.get_origin())
assert_mark_at(rotated_costmap, 29, 47, check_inflation=False)
# test binary map
costmap = CostMap2D.create_empty((5, 4), 0.05)
costmap.get_data()[30, 30] = CostMap2D.LETHAL_OBSTACLE
rotated_data = rotate_costmap(costmap.get_data(), -np.pi / 4)
non_free_indices = np.where(rotated_data != 0)
assert len(non_free_indices[0]) == 1
non_free_indices = (non_free_indices[0][0], non_free_indices[1][0])
assert non_free_indices == (47, 29)
assert (rotated_data[non_free_indices[0],
non_free_indices[1]] == CostMap2D.LETHAL_OBSTACLE)
def test_costmap_inflation():
"""Sanity checks on inflation"""
costmap = CostMap2D.create_empty((1, 1), 0.1, (0, 0))
add_wall_to_static_map(costmap, (0, 0), (1, 1))
cost_scaling_factor = 1.
inflated_costmap = inflate_costmap(costmap, cost_scaling_factor,
_rectangular_footprint())
inflated_pixels = len(
np.where(
inflated_costmap.get_data() == INSCRIBED_INFLATED_OBSTACLE)[0])
assert inflated_pixels == 70
def generate_trajectory_and_map_from_config(config):
""" Based on given MapConfig,
generate (trajectory to follow, CostMap2D) pair.
:param config MapConfig: map config based on which we will make a map.
:return (path_to_follow, CostMap2D) pair: path following task specification
"""
static_map = CostMap2D.create_empty(world_size=config.size,
resolution=config.resolution,
world_origin=config.origin)
for obs in config.obstacles:
static_map = obs.render(static_map)
return config.trajectory, static_map
def test_costmap_2d_extract_egocentric_binary_costmap():
costmap = CostMap2D.create_empty((5, 5), 0.05)
costmap.get_data()[30:32, 30:32] = CostMap2D.LETHAL_OBSTACLE
cut_map = extract_egocentric_costmap(costmap, (1.5, 1.5, -np.pi / 4),
resulting_origin=np.array(
[-2.0, -2.0], dtype=np.float64),
resulting_size=(4., 4))
rotated_data = cut_map.get_data()
non_free_indices = np.where(rotated_data != 0)
np.testing.assert_array_equal(non_free_indices[0], [40, 41, 41, 41, 42])
np.testing.assert_array_equal(non_free_indices[1], [40, 39, 40, 41, 40])
assert (
rotated_data[non_free_indices[0],
non_free_indices[1]] == CostMap2D.LETHAL_OBSTACLE).all()
def box_2d_planning(debug):
costmap = CostMap2D.create_empty((4, 4), 0.2, np.zeros((2,)))
gap = 1.2
add_wall_to_static_map(costmap, (0, 2), (2, 2), width=0.0)
add_wall_to_static_map(costmap, (2+gap, 2), (4, 2), width=0.0)
footprint_width = 0.79
footprint = np.array(
[[0.5*footprint_width, 0.5*footprint_width],
[-0.5 * footprint_width+1e-6, 0.5 * footprint_width],
[-0.5 * footprint_width+1e-6, -0.5 * footprint_width+1e-6],
[0.5 * footprint_width, -0.5 * footprint_width+1e-6]]
)
start_theta_discrete = 0
number_of_intermediate_states = 2
number_of_angles = 1
batch = []
action_cost_multiplier = 1
for i, end_cell in enumerate([[1, 0, 0],
[0, 1, 0],
[-1, 0, 0],
[0, -1, 0]]):
batch.append(create_linear_primitive(
primitive_id=i,
start_theta_discrete=start_theta_discrete,
action_cost_multiplier=action_cost_multiplier,
end_cell=end_cell,
number_of_intermediate_states=number_of_intermediate_states,
resolution=costmap.get_resolution(),
number_of_angles=number_of_angles))
motion_primitives = MotionPrimitives(
resolution=costmap.get_resolution(),
number_of_angles=1,
mprim_list=batch
)
start_pose = np.array([2.3, 1.3, 0.])
goal_pose = np.array([2.6, 2.8, 0.])
plan_xytheta, plan_xytheta_cell, controls, plan_time, solution_eps, environment = perform_single_planning(
planner_name='arastar',
footprint=footprint,
motion_primitives=motion_primitives,
forward_search=True,
costmap=costmap,
start_pose=start_pose,
goal_pose=goal_pose,
target_v=0.65,
target_w=1.0,
allocated_time=np.inf,
cost_scaling_factor=40.,
debug=False)
if debug:
print(environment.xytheta_real_to_cell(start_pose))
print(environment.xytheta_real_to_cell(goal_pose))
print(plan_xytheta_cell)
print("done with the solution of size=%d and sol. eps=%f" % (len(plan_xytheta_cell), solution_eps))
print("actual path (with intermediate poses) size=%d" % len(plan_xytheta))
params = environment.get_params()
costmap = environment.get_costmap()
img = prepare_canvas(costmap.shape)
draw_world_map_with_inflation(img, costmap)
for pose in plan_xytheta:
draw_robot(img, footprint, pose, params.cellsize_m, np.zeros((2,)),
color=70, color_axis=(1, 2))
draw_trajectory(img, params.cellsize_m, np.zeros((2,)), plan_xytheta)
draw_robot(img, footprint, start_pose, params.cellsize_m, np.zeros((2,)))
draw_robot(img, footprint, goal_pose, params.cellsize_m, np.zeros((2,)))
magnify = 16
img = cv2.resize(img, dsize=(0, 0), fx=magnify, fy=magnify, interpolation=cv2.INTER_NEAREST)
cv2.imshow("planning result", img)
cv2.waitKey(-1)
def test_is_robot_colliding():
costmap = CostMap2D.create_empty((10, 6), 0.05, (-1, -3))
wall_pos_x = 3.9
add_wall_to_static_map(costmap, (wall_pos_x, -4.), (wall_pos_x, -1 + 1.5))
wall_pos_x = 1.5
add_wall_to_static_map(costmap, (wall_pos_x, -4.), (wall_pos_x, -1 + 1.5))
add_wall_to_static_map(costmap, (5., -4.), (5. + 1, -1 + 4.5))
footprint = _rectangular_footprint()
poses = np.array([
(0., 0., 0.2),
(1., 0., 0.2),
(2., 0., 0.2),
(3., 0., 0.2),
(4., 0., 0.2),
(5., 0., 0.2),
(6., 0., 0.2),
(0., 1.2, np.pi / 2 + 0.4),
(1., 1.2, np.pi / 2 + 0.4),
(2., 1.2, np.pi / 2 + 0.4),
(3., 1.2, np.pi / 2 + 0.4),
(4., 1.2, np.pi / 2 + 0.4),
(5., 1.2, np.pi / 2 + 0.4),
(6., 1.2, np.pi / 2 + 0.4),
(0., -3, 0.2),
(1., -3, 0.2),
(2., -3, 0.2),
# These cases should be colliding but because of the current bug,
# they are not because robot's center is off bounds and we do not want to change behavior at this point
(0., -3.2, 0.2),
(1., -3.2, 0.2),
(2., -3.2, 0.2),
])
expected_collisions = [
False,
True,
True,
False,
True,
True,
True,
False,
False,
False,
False,
True,
True,
True,
False,
True,
True,
False,
False,
False,
]
assert len(poses) == len(expected_collisions)
for robot_pose, expected in zip(poses, expected_collisions):
is_colliding = is_robot_colliding(robot_pose, footprint,
costmap.get_data(),
costmap.get_origin(),
costmap.get_resolution())
assert is_colliding == expected
random_poses = np.random.rand(1000, 3)
random_poses[:, :2] *= costmap.world_size() + costmap.get_origin(
) + np.array([1., 1.])
for pose in random_poses:
is_colliding = is_robot_colliding(pose, footprint, costmap.get_data(),
costmap.get_origin(),
costmap.get_resolution())
is_colliding_ref = is_robot_colliding_reference(
pose, footprint, costmap.get_data(), costmap.get_origin(),
costmap.get_resolution())
assert is_colliding == is_colliding_ref
def path_and_costmap_from_config(params):
"""
Generate the actual path and turn
:param params TurnParams: information about the turn
:return: Tuple[ndarray(N, 3), Costmap2D]
"""
# we assume right turn, we can always flip it
turn_params = params.turn_params
hh = turn_params.main_corridor_length / 2
w = turn_params.turn_corridor_length / 2
alpha = turn_params.turn_corridor_angle
dd = turn_params.main_corridor_width
z = turn_params.turn_corridor_width
margin = turn_params.margin
flip_arnd_oy = turn_params.flip_arnd_oy
flip_arnd_ox = turn_params.flip_arnd_ox
rot_theta = turn_params.rot_theta
pts = _draw_pts_in_standard_coords(dd, hh, alpha, z, w)
oriented_way_pts = _generate_path_in_standard_coords(dd, hh, alpha, z, w)
# Maybe transform the points
rot_mtx = _rotation_matrix(rot_theta)
flipping_mtx = np.array([[-1. if flip_arnd_oy else 1., 0.],
[0., -1. if flip_arnd_ox else 1.]], )
transform_mtx = np.dot(rot_mtx, flipping_mtx)
new_pts = []
for pt in pts:
new_pt = np.dot(transform_mtx, pt)
new_pts.append(new_pt)
new_oriented_way_pts = []
for pt in oriented_way_pts:
x, y, t = pt
nx, ny = np.dot(transform_mtx, np.array([x, y]))
new_angle = t
if flip_arnd_ox:
new_angle = -new_angle
if flip_arnd_oy:
new_angle = np.pi - new_angle
new_angle = np.mod(new_angle + rot_theta, 2 * np.pi)
new_pt = np.array([nx, ny, new_angle])
new_oriented_way_pts.append(new_pt)
a, _, c, d, e, _, g, h, i, j = new_pts # pylint: disable=unbalanced-tuple-unpacking
rb, rk, rl, rf = new_oriented_way_pts # pylint: disable=unbalanced-tuple-unpacking
all_pts = np.array(list(new_pts))
min_x = all_pts[:, 0].min()
max_x = all_pts[:, 0].max()
min_y = all_pts[:, 1].min()
max_y = all_pts[:, 1].max()
world_size = abs(max_x - min_x) + 2 * margin, abs(max_y -
min_y) + 2 * margin
world_origin = min_x - margin, min_y - margin
obstacles = [
Wall(from_pt=a, to_pt=i),
Wall(from_pt=c, to_pt=d),
Wall(from_pt=d, to_pt=e),
Wall(from_pt=j, to_pt=g),
Wall(from_pt=g, to_pt=h)
]
static_path = np.array([rb, rk, rl, rf])
static_map = CostMap2D.create_empty(
world_size=world_size, # x width, y height
resolution=params.env_params.resolution,
world_origin=world_origin)
for obs in obstacles:
static_map = obs.render(static_map)
return static_path, static_map