def draw_robot(image_to_draw,
footprint,
pose,
resolution,
origin,
color=(30, 150, 30),
color_axis=None,
fill=True):
"""
Print robot on an image
:param image_to_draw: image to draw on
:param footprint: footprint of the robot to print
:param pose: pose of the robot
:param resolution: costmap resoliuoonc
:param origin: origin of the costmap
:param color: color of the robot to draw
:param color_axis: color of the axis to draw
:param fill: should we fill
:return: px, py where the robot is on the picture
"""
px, py = get_drawing_coordinates_from_physical(
image_to_draw.shape, resolution, origin,
np.array(pose[0:2], dtype=np.float64))
kernel = get_pixel_footprint_for_drawing(pose[2],
footprint,
resolution,
fill=fill)
blit(kernel, image_to_draw, px, py, color, axis=color_axis)
return px, py
def test_blit():
'''
Test blitting corner cases - blit half outside costmap, fully outside, etc
'''
for depth in (1, 3):
if depth == 1:
im = np.zeros((20, 50), dtype=np.uint8)
else:
im = np.zeros((20, 50, 3), dtype=np.uint8)
patch = np.zeros((6, 4), dtype=np.uint8)
patch[2:4, :] = 255
for ((x, y), xlim, ylim) in [((10, 10), (8, 12), (9, 11)),
((0, 10), (0, 2), (9, 11)),
((50, 10), (48, 50), (9, 11)),
((10, 0), (8, 12), (0, 1)),
((10, 20), (8, 12), (19, 20)),
((0, 0), (0, 2), (0, 1)),
((50, 20), (48, 50), (19, 20)),
((0, 20), (0, 2), (19, 20)),
((50, 0), (48, 50), (0, 1)),
((-1, 20), (0, 1), (19, 20)),
((51, 0), (49, 50), (0, 1)),
((10, -1), None, None),
((10, 21), None, None),
((-2, 10), None, None),
((52, 10), None, None),
((-100, 10), None, None),
((200, 10), None, None),
((200, 200), None, None),
((10, -100), None, None),
((-5, -5), None, None)] +\
[((x, y), None, None) for y in range(22, 50) for x in [-10, 60, 3]] + \
[((x, y), None, None) for y in range(-30, 0) for x in [-10, 60, 3]] + \
[((x, y), None, None) for x in range(52, 100) for y in [-10, 30, 2]] + \
[((x, y), None, None) for x in range(-30, -1) for y in [-10, 30, 2]]:
blit(patch, im, x, y, 100)
if xlim is None or ylim is None:
assert np.all(im == 0)
assert np.sum(get_blit_values(patch, im, x, y)) == 0
else:
assert np.all(im[ylim[0]:ylim[1], xlim[0]:xlim[1], ...] == 100)
assert np.sum(im) == depth * 100 * (ylim[1] - ylim[0]) * (
xlim[1] - xlim[0])
assert np.sum(get_blit_values(patch, im, x, y)) == np.sum(im)
im[:] = 0
# Finally test a blit bigger than the image
im = np.zeros((3, 3), dtype=np.uint8)
patch = np.zeros((5, 5), dtype=np.uint8)
patch[2:4, :] = 255
x, y = 1, 1
xlim = (0, 3)
ylim = (1, 3)
blit(patch, im, x, y, 100)
assert np.all(im[ylim[0]:ylim[1], xlim[0]:xlim[1], ...] == 100)
assert np.sum(im) == 100 * (ylim[1] - ylim[0]) * (xlim[1] - xlim[0])
assert np.sum(get_blit_values(patch, im, x, y)) == np.sum(im)
def draw(self,
image,
px,
py,
angle,
color,
map_resolution,
alpha=1.0,
draw_steering_details=True):
"""
Draw robot on the image
:param image: cv image to draw on
:param px: pixel coordinates of the robot
:param py: pixel coordinates of the robot
:param angle: angle of the robot in drawing coordinates
:param color: color to draw with
:param map_resolution: resolution of the image
:param alpha float: transparency of the robot image
:param draw_steering_details bool: Should draw state of steering on the image
"""
# get_pixel_footprint_for_drawing takes angle in physical coordinates
kernel = get_pixel_footprint_for_drawing(
get_physical_angle_from_drawing(angle), self.get_footprint(),
map_resolution)
blit(kernel, image, px, py, color, alpha=alpha)
arrow_length = 15
draw_arrow(image, (px, py), (px + int(arrow_length * np.cos(angle)),
py + int(arrow_length * np.sin(angle))),
color=(0, 0, 255),
thickness=2)
wheel_pixel_x = int(px + np.cos(angle) *
self.get_front_wheel_from_axis_distance() /
map_resolution)
wheel_pixel_y = int(py + np.sin(angle) *
self.get_front_wheel_from_axis_distance() /
map_resolution)
if draw_steering_details:
draw_arrow(
image, (wheel_pixel_x, wheel_pixel_y),
(wheel_pixel_x +
int(arrow_length * np.cos(angle - self._state.wheel_angle)),
wheel_pixel_y +
int(arrow_length * np.sin(angle - self._state.wheel_angle))),
color=(0, 0, 255),
thickness=1)