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 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 get_random_maps_squeeze_between_obstacle_in_corridor_on_path():
"""
Download (or read from cache) randomly edited real world map with of a robot
in a corridor with 3 square obstacles
:return: a tuple of 3 elements
- original realworld costmap of the corridor with 3 boxes
- reference path that human took
- tuple of 1000 randomized maps that were obstained by cutting and pasting 3 boxes randomly around
their original localization
"""
key = '413293a9-086b-46e0-83ef-bb09d23c70d5'
test_maps_cache = 'test_random_maps_%s.pkl' % key
session_tar_key = 'tdwa_paper_' + os.path.splitext(
test_maps_cache)[0] + '.tar.xz'
session_tar_file = get_cache_key_path(session_tar_key)
tar_contents_folder = session_tar_file + '.contents/'
if not os.path.exists(session_tar_file):
r = requests.get(
'https://s3-us-west-1.amazonaws.com/braincorp-research-public-datasets/'
+ session_tar_key)
with open(session_tar_file, 'wb') as f:
f.write(r.content)
# remove unzipped cache if redownloaded
if os.path.exists(tar_contents_folder):
shutil.rmtree(tar_contents_folder)
if not os.path.exists(tar_contents_folder):
print('Unzipping Tar file "%s"' % session_tar_file)
try:
decompress_tar_archive(session_tar_file,
tar_contents_folder,
verbose=True)
except Exception as ex:
shutil.rmtree(tar_contents_folder)
raise ex
with open(os.path.join(tar_contents_folder, test_maps_cache), 'rb') as f:
if sys.version_info > (3, 0):
original_costmap = CostMap2D.from_state(
pickle.load(f, encoding='latin1')) # pylint: disable=unexpected-keyword-arg
static_path = pickle.load(f, encoding='latin1') # pylint: disable=unexpected-keyword-arg
test_maps = tuple([
CostMap2D.from_state(s)
for s in pickle.load(f, encoding='latin1')
]) # pylint: disable=unexpected-keyword-arg
else:
original_costmap = CostMap2D.from_state(pickle.load(f))
static_path = pickle.load(f)
test_maps = tuple(
[CostMap2D.from_state(s) for s in pickle.load(f)])
return original_costmap, static_path, test_maps
def deserialize(cls, state):
ver = state.pop('version')
assert ver == cls.VERSION
state['costmap'] = CostMap2D.from_state(state['costmap'])
reward_provider_state_instance = create_reward_provider_state(
state.pop('reward_provider_state_name'))
state[
'reward_provider_state'] = reward_provider_state_instance.deserialize(
state['reward_provider_state'])
# prepare for robot state deserialization
robot_instance = create_standard_robot(state.pop('robot_type_name'))
robot_state_type = robot_instance.get_state_type()
# deserialize the robot state
state['robot_state'] = robot_state_type.deserialize(
state['robot_state'])
# deserialize robot state queue
acc = []
for item in state['robot_state_queue']:
acc.append(robot_state_type.deserialize(item))
state['robot_state_queue'] = acc
return cls(**state)
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 clone_costmap(costmap):
'''
Clone other costmap copying the data and other fields
:param costmap CostMap2D: costmap to clone
:return CostMap2D: costmap with copied data
'''
return CostMap2D(costmap.get_data().copy(),
costmap.get_resolution(),
costmap.get_origin().copy())
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 deserialize(cls, state):
ver = state.pop('version')
assert ver == cls.VERSION
state['costmap'] = CostMap2D.from_state(state['costmap'])
robot_instance = create_standard_robot(state.pop('robot_type_name'))
robot_state_type = robot_instance.get_state_type()
# deserialize the robot state
state['robot_state'] = robot_state_type.deserialize(
state['robot_state'])
return cls(**state)
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 deserialize(cls, state):
ver = state.pop('version')
assert ver == cls.VERSION
init_costmap = CostMap2D.from_state(state['costmap'])
init_path = state['path']
params = EnvParams.deserialize(state['params'])
state = State.deserialize(state['state'])
instance = cls(init_costmap, init_path, params)
instance.set_state(state)
return instance
def extract_egocentric_costmap(costmap_2d, ego_position_in_world,
resulting_origin=None,
resulting_size=None, border_value=0):
"""
Returns a costmap as seen by robot at ego_position_in_world.
In this costmap robot is at (0, 0) with 0 angle.
:param costmap_2d: global costmap
:param ego_position_in_world: robot's position in the world costmap
:param resulting_origin: Perform additional shifts and cuts so that
resulting costmap origin and world size are equal to those parameters
:param resulting_size: Perform additional shifts and cuts so that
resulting costmap origin and world size are equal to those parameters
:param border_value: the value at the border of the costmap to extrapolate
:return: returns the egocentric costmap, with the robot in the middle.
"""
ego_pose = np.asarray(ego_position_in_world)
pixel_origin = costmap_2d.world_to_pixel(np.array(ego_pose[:2], dtype=np.float64))
transform = cv2.getRotationMatrix2D(tuple(pixel_origin), 180 * ego_pose[2] / np.pi, scale=1)
if resulting_size is None:
resulting_size = costmap_2d.get_data().shape[:2][::-1]
else:
resulting_size = tuple(world_to_pixel(np.array(resulting_size, dtype=np.float64), np.array((0., 0.)), costmap_2d.get_resolution()))
if resulting_origin is not None:
resulting_origin = np.asarray(resulting_origin)
assert resulting_origin.shape[0] == 2
delta_shift = resulting_origin - (costmap_2d.get_origin() - ego_pose[:2])
delta_shift_pixel = tuple(world_to_pixel(delta_shift, np.array((0., 0.)), costmap_2d.get_resolution()))
shift_matrix = np.float32([[1, 0, -delta_shift_pixel[0]], [0, 1, -delta_shift_pixel[1]]])
def _compose_affine_transforms(t1, t2):
# http://stackoverflow.com/questions/13557066/built-in-function-to-combine-affine-transforms-in-opencv
t1_expanded = np.array((t1[0], t1[1], (0, 0, 1)), dtype=np.float32)
t2_expanded = np.array((t2[0], t2[1], (0, 0, 1)), dtype=np.float32)
combined = np.dot(t2_expanded, t1_expanded)
return combined[:2, :]
transform = _compose_affine_transforms(transform, shift_matrix)
else:
resulting_origin = costmap_2d.get_origin() - ego_pose[:2]
with single_threaded_opencv():
rotated_data = cv2.warpAffine(costmap_2d.get_data(), transform, resulting_size,
# this mode doesn't change the value of pixels during rotation
flags=cv2.INTER_NEAREST, borderValue=border_value)
return CostMap2D(rotated_data, costmap_2d.get_resolution(),
origin=resulting_origin)
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_costmap_with_nonzero_origin():
costmap2d = CostMap2D(np.zeros((100, 100), dtype=np.float64), 0.05,
np.array([1.0, 2.0]))
costmap2d.get_data()[10, 20] = CostMap2D.LETHAL_OBSTACLE
# rotate, shift and cut -> extract ego costmap centered on the robot
cut_map = extract_egocentric_costmap(costmap2d, (3.5, 3.5, -np.pi / 4),
resulting_origin=np.array(
[-2, -2], dtype=np.float64),
resulting_size=(4, 4))
assert cut_map.get_resolution() == costmap2d.get_resolution()
np.testing.assert_array_equal(cut_map.get_origin(), (-2, -2))
np.testing.assert_array_equal(cut_map.get_data().shape, (80, 80))
np.testing.assert_array_almost_equal(cut_map.world_bounds(),
(-2, 2, -2, 2))
assert_mark_at(cut_map, 33, 5)
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 inflate_costmap(costmap, cost_scaling_factor, footprint):
'''
Inflate data with costs
Costmap inflation is explained here:
http://wiki.ros.org/costmap_2d#Inflation
:param costmap CostMap2D: obstacle map
:param cost_scaling_factor Float: how to scale the inflated costs
:param footprint np.ndarray(N, 2)[Float]: footprint of the robot
:return CostMap2D: inflated costs
'''
image_for_dist_transform = CostMap2D.LETHAL_OBSTACLE - costmap.get_data()
distance = distance_transform(image_for_dist_transform)
inscribed_rad = inscribed_radius(footprint)
inflated_data = _pixel_distance_to_cost(distance, costmap.get_resolution(),
inscribed_rad, cost_scaling_factor)
return CostMap2D(data=inflated_data,
origin=costmap.get_origin(),
resolution=costmap.get_resolution())
def visualize_in_gym(gym_env, exp_map, path, pose):
"""
Alternative visualization method using gym rendering function
:param gym_env RandomAisleTurnEnv: object storing the robot and environment state in bc_gym_planning_env standard
:param exp_map Costmap: occupancy map in bc_exploration standard
:param path array(N, 3)[float]: containing an array of poses for the robot to follow
:param pose array(3)[float]: corresponds to the pose of the robot [x, y, theta]
"""
gym_compat_map = np.zeros(exp_map.data.shape, dtype=np.uint8)
gym_compat_map[exp_map.data ==
Costmap.OCCUPIED] = CostMap2D.LETHAL_OBSTACLE
gym_compat_map[exp_map.data == Costmap.FREE] = CostMap2D.FREE_SPACE
gym_compat_map[exp_map.data ==
Costmap.UNEXPLORED] = CostMap2D.NO_INFORMATION
gym_costmap = CostMap2D(np.flipud(gym_compat_map), exp_map.resolution,
exp_map.origin.copy())
gym_state = gym_env.get_state()
if np.array(path).shape[0]:
gym_state.path = path
gym_state.original_path = path
else:
gym_state.path = pose[None, :]
gym_state.original_path = pose[None, :]
gym_state.costmap = gym_costmap
gym_state.pose = pose
gym_state.robot_state.x = pose[0]
gym_state.robot_state.y = pose[1]
gym_state.robot_state.angle = pose[2]
gym_env.set_state(gym_state)
gym_env.render(mode='human')
def test_draw_trajectory():
costmap = CostMap2D(data=np.zeros((400, 400), dtype=np.uint8),
origin=np.array([-10., -10.]),
resolution=0.05)
trajectory_picture = np.zeros((costmap.get_data().shape + (3, )),
dtype=np.uint8)
trajectory = np.array([[-5, -2, np.pi / 4], [-4.5, -1.5, np.pi / 3]])
draw_trajectory(trajectory_picture,
costmap.get_resolution(),
costmap.get_origin(),
trajectory,
color=(0, 255, 0))
idy, idx = np.where(np.all(trajectory_picture == (0, 255, 0), axis=2))
np.testing.assert_array_equal(
idy, [229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239])
np.testing.assert_array_equal(
idx, [110, 109, 108, 107, 106, 105, 104, 103, 102, 101, 100])
draw_trajectory(trajectory_picture, costmap.get_resolution(),
costmap.get_origin(), [])
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 test_costmap_2d_extract_egocentric_costmap():
costmap2d = CostMap2D(np.zeros((100, 100), dtype=np.float64), 0.05,
np.array([0.0, 0.0]))
costmap2d.get_data()[10, 20] = CostMap2D.LETHAL_OBSTACLE
np.testing.assert_array_equal(costmap2d.world_bounds(), (0, 5, 0, 5))
assert_mark_at(costmap2d, 20, 10)
# # dummy cut
cut_map = extract_egocentric_costmap(costmap2d, (0., 0., 0.))
assert cut_map.get_resolution() == costmap2d.get_resolution()
np.testing.assert_array_equal(cut_map.get_origin(), (0.0, 0.0))
np.testing.assert_array_equal(cut_map.get_data().shape, (100, 100))
np.testing.assert_array_equal(cut_map.world_bounds(), (0.0, 5, 0.0, 5))
assert_mark_at(cut_map, 20, 10)
# shift
cut_map = extract_egocentric_costmap(costmap2d, (0.2, 0.2, 0.0))
assert cut_map.get_resolution() == costmap2d.get_resolution()
np.testing.assert_array_equal(cut_map.get_origin(), (-0.2, -0.2))
np.testing.assert_array_equal(cut_map.get_data().shape, (100, 100))
np.testing.assert_array_equal(cut_map.world_bounds(),
(-0.2, 4.8, -0.2, 4.8))
# ego centric view doesn't shift the data itself if not rotated
assert_mark_at(cut_map, 20, 10)
# rotate so that mark is almost in the front of the robot
cut_map = extract_egocentric_costmap(costmap2d,
(0.0, 0.0, np.pi / 6 - 0.05))
assert cut_map.get_resolution() == costmap2d.get_resolution()
np.testing.assert_array_equal(cut_map.get_origin(), (-0., -0.))
np.testing.assert_array_equal(cut_map.get_data().shape, (100, 100))
np.testing.assert_array_equal(cut_map.world_bounds(), (0.0, 5, 0.0, 5))
assert_mark_at(cut_map, 22, 0, check_single=False)
# rotate as if robot is in the center of the costmap
cut_map = extract_egocentric_costmap(costmap2d, (2.5, 2.5, -np.pi / 2.))
assert cut_map.get_resolution() == costmap2d.get_resolution()
np.testing.assert_array_equal(cut_map.get_origin(), (-2.5, -2.5))
np.testing.assert_array_equal(cut_map.get_data().shape, (100, 100))
np.testing.assert_array_equal(cut_map.world_bounds(),
(-2.5, 2.5, -2.5, 2.5))
assert_mark_at(cut_map, 90, 20)
# rotate as if robot is in the center of the costmap with explicit parameters
cut_map = extract_egocentric_costmap(costmap2d, (2.5, 2.5, -np.pi / 2),
resulting_origin=np.array(
[-2.5, -2.5], dtype=np.float64),
resulting_size=np.array((5., 5.)))
assert cut_map.get_resolution() == costmap2d.get_resolution()
np.testing.assert_array_equal(cut_map.get_origin(), (-2.5, -2.5))
np.testing.assert_array_equal(cut_map.get_data().shape, (100, 100))
np.testing.assert_array_equal(cut_map.world_bounds(),
(-2.5, 2.5, -2.5, 2.5))
assert_mark_at(cut_map, 90, 20)
# rotate as if robot is in the center of the costmap with smaller size
cut_map = extract_egocentric_costmap(costmap2d, (2.5, 2.5, -np.pi / 2),
resulting_origin=np.array(
[-2.5, -2.5], dtype=np.float64),
resulting_size=(4.6, 4.9))
assert cut_map.get_resolution() == costmap2d.get_resolution()
np.testing.assert_array_equal(cut_map.get_origin(), (-2.5, -2.5))
np.testing.assert_array_equal(cut_map.get_data().shape, (98, 92))
np.testing.assert_array_almost_equal(cut_map.world_bounds(),
(-2.5, 2.1, -2.5, 2.4))
assert_mark_at(cut_map, 90, 20)
# do not rotate but shift
cut_map = extract_egocentric_costmap(costmap2d, (2.5, 2.5, 0.0),
resulting_origin=np.array(
[-5, -4], dtype=np.float64),
resulting_size=(5, 5))
assert cut_map.get_resolution() == costmap2d.get_resolution()
np.testing.assert_array_equal(cut_map.get_origin(), (-5, -4))
np.testing.assert_array_equal(cut_map.get_data().shape, (100, 100))
np.testing.assert_array_almost_equal(cut_map.world_bounds(),
(-5, 0, -4, 1))
assert_mark_at(cut_map, 70, 40)
# do not rotate but shift and cut
cut_map = extract_egocentric_costmap(costmap2d, (2.5, 2.5, 0.0),
resulting_origin=np.array(
[-5, -4], dtype=np.float64),
resulting_size=(4, 4))
assert cut_map.get_resolution() == costmap2d.get_resolution()
np.testing.assert_array_equal(cut_map.get_origin(), (-5, -4))
np.testing.assert_array_equal(cut_map.get_data().shape, (80, 80))
np.testing.assert_array_almost_equal(cut_map.world_bounds(),
(-5, -1, -4, 0))
assert_mark_at(cut_map, 70, 40)
# rotate, shift and cut -> extract ego costmap centered on the robot
cut_map = extract_egocentric_costmap(costmap2d, (1.5, 1.5, -np.pi / 4),
resulting_origin=np.array(
[-2, -2], dtype=np.float64),
resulting_size=(4, 4))
assert cut_map.get_resolution() == costmap2d.get_resolution()
np.testing.assert_array_equal(cut_map.get_origin(), (-2, -2))
np.testing.assert_array_equal(cut_map.get_data().shape, (80, 80))
np.testing.assert_array_almost_equal(cut_map.world_bounds(),
(-2, 2, -2, 2))
assert_mark_at(cut_map, 47, 19)
# rotate, shift and expand -> extract ego costmap centered on the robot, going off the limits of original map
cut_map = extract_egocentric_costmap(costmap2d, (1., 1.5, -np.pi / 4),
resulting_origin=np.array(
[-3, -3], dtype=np.float64),
resulting_size=(7, 6))
assert cut_map.get_resolution() == costmap2d.get_resolution()
np.testing.assert_array_equal(cut_map.get_origin(), (-3, -3))
np.testing.assert_array_equal(cut_map.get_data().shape, (120, 140))
np.testing.assert_array_almost_equal(cut_map.world_bounds(),
(-3, 4, -3, 3))
assert_mark_at(cut_map, 74, 46)
costmap_with_image = CostMap2D(np.zeros((100, 100, 3), dtype=np.float64),
0.05, np.array([0., 0.]))
extract_egocentric_costmap(costmap_with_image, [0., 0., np.pi / 4])
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
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)