def pose_collides(x, y, angle, robot, costmap):
"""
Check if robot footprint at x, y (world coordinates) and
oriented as yaw collides with lethal obstacles.
:param x: robot pose
:param y: robot pose
:param angle: robot pose
:param robot: Robot that will supply the footprint
:param costmap Costmap2D: costmap containing the obstacles to collide with
:return bool : does the pose collide?
"""
kernel_image = get_pixel_footprint(angle, robot.get_footprint(), costmap.get_resolution())
# Get the coordinates of where the footprint is inside the kernel_image (on pixel coordinates)
kernel = np.where(kernel_image)
# Move footprint to (x,y), all in pixel coordinates
x, y = world_to_pixel(np.array([x, y]), costmap.get_origin(), costmap.get_resolution())
collisions = y + kernel[0] - kernel_image.shape[0] // 2, x + kernel[1] - kernel_image.shape[1] // 2
raw_map = costmap.get_data()
# Check if the footprint pixel coordinates are valid, this is, if they are not negative and are inside the map
good = np.logical_and(np.logical_and(collisions[0] >= 0, collisions[0] < raw_map.shape[0]),
np.logical_and(collisions[1] >= 0, collisions[1] < raw_map.shape[1]))
# Just from the footprint coordinates that are good, check if they collide
# with obstacles inside the map
return bool(np.any(raw_map[collisions[0][good],
collisions[1][good]] == CostMap2D.LETHAL_OBSTACLE))
def is_robot_colliding_reference(robot_pose, footprint, costmap_data, origin,
resolution):
"""
Pure python implementation of is_robot_colliding
Check costmap for obstacle at (world_x, world_y)
If orientation is None, obstacle detection will check only the inscribed-radius
distance collision. This means that, if the robot is not circular,
there may be an undetected orientation-dependent collision.
If orientation is given, footprint must also be given, and should be the same
used by the costmap to inflate costs. Proper collision detection will
then be done.
"""
assert isinstance(robot_pose, np.ndarray)
# convert to costmap coordinate system:
map_x, map_y = world_to_pixel(robot_pose[:2], origin, resolution)
# TODO: remove this because the following is technically wrong: even if robot's origin is outside, it still can collide
if not in_costmap_bounds(costmap_data, map_x, map_y):
return False
cost = costmap_data[map_y, map_x]
if cost in [CostMap2D.LETHAL_OBSTACLE]:
return True
# Now check for orientation-dependent collision
fp = get_pixel_footprint(robot_pose[2], footprint, resolution)
values = get_blit_values(fp, costmap_data, map_x, map_y)
return (values == CostMap2D.LETHAL_OBSTACLE).any()
def test_get_pixel_footprint_consistency():
footrpints = [_rectangular_footprint(), _tricycle_footprint()]
for fill in [True, False]:
for resolution in [0.03, 0.05]:
for f in footrpints:
angles = np.random.random_sample(1000) * np.pi * 2 - np.pi
for angle in angles:
pixel_footprint = get_pixel_footprint(angle,
f,
resolution,
fill=fill)
reference = get_pixel_footprint_py(angle,
f,
resolution,
fill=fill)
np.testing.assert_array_equal(pixel_footprint, reference)
def get_pixel_footprint_for_drawing(angle,
robot_footprint,
map_resolution,
fill=True):
"""
Return pixel footprint kernel for visualization of the robot.
The footprint kernel is flipped.
angle_range - angle in physical coordinates (!)
:param angle: the angle
:param robot_footprint: robots foot print
:param map_resolution: resolution of the map
:param fill: shuold we fill
:return: picture of the footprint
"""
footprint_picture = get_pixel_footprint(angle, robot_footprint,
map_resolution, fill)
footprint_picture = np.flipud(footprint_picture)
return footprint_picture
def _override_primitive_kernels(self, motion_primitives, footprint,
use_full_kernels):
resolution = motion_primitives.get_resolution()
print('Setting up motion primitive kernels..')
for p in motion_primitives.get_primitives():
primitive_start = pixel_to_world(np.zeros((2, )), np.zeros((2, )),
resolution)
primitive_states = p.get_intermediate_states().copy()
primitive_states[:, :2] += primitive_start
full_cv_kernel_x = []
full_cv_kernel_y = []
for pose in primitive_states:
kernel = get_pixel_footprint(pose[2], footprint, resolution)
kernel_center = (kernel.shape[1] // 2, kernel.shape[0] // 2)
kernel = np.where(kernel)
px, py = world_to_pixel(pose[:2], np.zeros((2, )), resolution)
full_cv_kernel_x.append(kernel[1] + (px - kernel_center[0]))
full_cv_kernel_y.append(kernel[0] + (py - kernel_center[1]))
full_cv_kernel_x = np.hstack(full_cv_kernel_x)
full_cv_kernel_y = np.hstack(full_cv_kernel_y)
full_cv_kernel = np.column_stack(
(full_cv_kernel_x, full_cv_kernel_y)).astype(np.int32)
row_view = np.ascontiguousarray(full_cv_kernel).view(
np.dtype(
(np.void,
full_cv_kernel.dtype.itemsize * full_cv_kernel.shape[1])))
_, idx = np.unique(row_view, return_index=True)
full_cv_kernel = np.ascontiguousarray(full_cv_kernel[idx])
if use_full_kernels:
self.set_primitive_collision_pixels(p.starttheta_c,
p.motprimID,
full_cv_kernel)
else:
min_x, max_x = np.amin(full_cv_kernel[:, 0]), np.amax(
full_cv_kernel[:, 0])
min_y, max_y = np.amin(full_cv_kernel[:, 1]), np.amax(
full_cv_kernel[:, 1])
temp_img = np.zeros((max_y - min_y + 1, max_x - min_x + 1),
dtype=np.uint8)
temp_img[full_cv_kernel[:, 1] - min_y,
full_cv_kernel[:, 0] - min_x] = 255
_, contours, _ = cv2.findContours(temp_img.copy(),
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
contour = contours[0].reshape(-1, 2)
perimeter_kernel = np.column_stack(
(contour[:, 0] + min_x,
contour[:, 1] + min_y)).astype(np.int32)
self.set_primitive_collision_pixels(p.starttheta_c,
p.motprimID,
perimeter_kernel)
print('Done.')