racecar_utils Library

The racecar_utils library provides helper functions which aid in performing common robotics tasks. Many of these functions are designed to interface with the inputs and outputs of the racecar_core library.

class racecar_utils.ColorBGR(value)

Common colors defined in the blue-green-red (BGR) format, with each channel ranging from 0 to 255 inclusive.

class racecar_utils.Direction(value)

AR marker direction

racecar_utils.clamp(value, min, max)

Clamps a value between a minimum and maximum value.

Parameters
  • value (float) – The input to clamp.

  • min (float) – The minimum allowed value.

  • max (float) – The maximum allowed value.

Return type

float

Returns

The value saturated between min and max.

Example:

# a will be set to 3
a = rc_utils.clamp(3, 0, 10)

# b will be set to 0
b = rc_utils.remap_range(-2, 0, 10)

# c will be set to 10
c = rc_utils.remap_range(11, 0, 10)
racecar_utils.colormap_depth_image(depth_image, max_depth=1000)

Converts a depth image to a colored image representing depth.

Parameters
  • depth_image (NDArray[(typing.Any, typing.Any), float32]) – The depth image to convert.

  • max_depth (int) – The farthest depth to show in the image in cm. Anything past this depth is shown as the farthest color.

Return type

NDArray[(typing.Any, typing.Any, 3), uint8]

Returns

A color image representation of the provided depth image.

Note

Each color value ranges from 0 to 255. The color of each pixel is determined by its distance.

Example::

# retrieve a depth image depth_image = rc.camera.get_depth_image()

# get the colormapped depth image depth_image_colormap = rc_utils.colormap_depth_image(depth_image)

racecar_utils.crop(image, top_left_inclusive, bottom_right_exclusive)

Crops an image to a rectangle based on the specified pixel points.

Parameters
  • image (NDArray[(typing.Any, Ellipsis), Any]) – The color or depth image to crop.

  • top_left_inclusive (Tuple[float, float]) – The (row, column) of the top left pixel of the crop rectangle.

  • bottom_right_exclusive (Tuple[float, float]) – The (row, column) of the pixel one past the bottom right corner of the crop rectangle.

Return type

NDArray[(typing.Any, Ellipsis), Any]

Returns

A cropped version of the image.

Note

The top_left_inclusive pixel is included in the crop rectangle, but the bottom_right_exclusive pixel is not.

If bottom_right_exclusive exceeds the bottom or right edge of the image, the full image is included along that axis.

Example:

image = rc.camera.get_color_image()

# Crop the image to only keep the top half
cropped_image = rc_utils.crop(
    image, (0, 0), (rc.camera.get_height() // 2, rc.camera.get_width())
)
racecar_utils.draw_ar_markers(color_image, corners, ids, color=0, 255, 0)

Draw AR markers in a image.

Parameters
  • color_image (NDArray[(typing.Any, typing.Any, 3), uint32]) – A color image.

  • corners (List[NDArray[(1, 4, 2), int32]]) – A list of ndarrays with AR marker corners.

  • ids (NDArray[(typing.Any, 1), int32]) – A list of AR marker ids.

Note

The length of corners must be the same as the first dimension of ids. The original image is modified.

Return type

NDArray[(typing.Any, typing.Any, 3), uint8]

Returns

A list of each AR marker’s four corners clockwise and an array of the ar marker ids.

Example::

color_image = copy.deepcopy(rc.camera.get_color_image())

# detect and draw AR markers corners, ids = racecar_utils.get_ar_markers(color_image) color_image = racecar_utils.draw_ar_markers(color_image, corners, ids)

rc.display.show_color_image(color_image)

racecar_utils.draw_circle(color_image, center, color=0, 255, 255, radius=6)

Draws a circle on the provided image.

Parameters
  • color_image (NDArray[(typing.Any, typing.Any, 3), uint8]) – The color image on which to draw the contour.

  • center (Tuple[int, int]) – The pixel (row, column) of the center of the image.

  • color (Tuple[int, int, int]) – The color to draw the circle, specified as blue-green-red channels each ranging from 0 to 255 inclusive.

  • radius (int) – The radius of the circle in pixels.

Example:

image = rc.camera.get_color_image()

# Extract the largest blue contour
BLUE_HSV_MIN = (90, 50, 50)
BLUE_HSV_MAX = (110, 255, 255)
contours = rc_utils.find_contours(image, BLUE_HSV_MIN, BLUE_HSV_MAX)
largest_contour = rc_utils.get_largest_contour(contours)

# Draw a dot at the center of this contour in red
if (largest_contour is not None):
    center = get_contour_center(contour)
    draw_circle(image, center, rc_utils.ColorBGR.red.value)
Return type

None

racecar_utils.draw_contour(color_image, contour, color=0, 255, 0)

Draws a contour on the provided image.

Parameters
  • color_image (NDArray[(typing.Any, typing.Any, 3), uint8]) – The color image on which to draw the contour.

  • contour (NDArray) – The contour to draw on the image.

  • color (Tuple[int, int, int]) – The color to draw the contour, specified as blue-green-red channels each ranging from 0 to 255 inclusive.

Example:

image = rc.camera.get_color_image()

# Extract the largest blue contour
BLUE_HSV_MIN = (90, 50, 50)
BLUE_HSV_MAX = (110, 255, 255)
contours = rc_utils.find_contours(image, BLUE_HSV_MIN, BLUE_HSV_MAX)
largest_contour = rc_utils.get_largest_contour(contours)

# Draw this contour onto image
if (largest_contour is not None):
    draw_contour(image, largest_contour)
Return type

None

racecar_utils.find_contours(color_image, hsv_lower, hsv_upper)

Finds all contours of the specified color range in the provided image.

Parameters
  • color_image (NDArray[(typing.Any, typing.Any, 3), uint8]) – The color image in which to find contours, with pixels represented in the bgr (blue-green-red) format.

  • hsv_lower (Tuple[int, int, int]) – The lower bound for the hue, saturation, and value of colors to contour.

  • hsv_upper (Tuple[int, int, int]) – The upper bound for the hue, saturation, and value of the colors to contour.

Return type

List[NDArray]

Returns

A list of contours around the specified color ranges found in color_image.

Note

Each channel in hsv_lower and hsv_upper ranges from 0 to 255.

Example:

# Define the lower and upper hsv ranges for the color blue
BLUE_HSV_MIN = (90, 50, 50)
BLUE_HSV_MAX = (110, 255, 255)

# Extract contours around all blue portions of the current image
contours = rc_utils.find_contours(
    rc.camera.get_color_image(), BLUE_HSV_MIN, BLUE_HSV_MAX
)
racecar_utils.get_ar_direction(ar_corners)

Return the direction of an AR marker.

Parameters

ar_corners (NDArray[(1, 4, 2), int32]) – An array of the AR marker’s corner coordinates.

Note

The first dimension of the input array is 1.

Return type

Direction

Returns

The direction the AR markers faces.

Example::

color_image = copy.deepcopy(rc.camera.get_color_image())

# detect AR marker and print direction corners, ids = racecar_utils.get_ar_markers(color_image) if len(corners) > 0:

print(get_ar_direction(corners[0]))

racecar_utils.get_ar_markers(color_image)

Finds AR markers in a image.

Parameters

color_image (NDArray[(typing.Any, typing.Any, 3), uint8]) – A color image.

Return type

Tuple[List[NDArray[(1, 4, 2), int32]], Optional[NDArray[(typing.Any, 1), int32]]]

Returns

A list of each AR marker’s four corners clockwise and an array of the AR marker ids.

Example::

color_image = copy.deepcopy(rc.camera.get_color_image())

# detect and draw AR markers corners, ids = racecar_utils.get_ar_markers(color_image) color_image = racecar_utils.draw_ar_markers(color_image, corners, ids)

rc.display.show_color_image(color_image)

racecar_utils.get_closest_pixel(depth_image, kernel_size=5)

Finds the closest pixel in a depth image.

Parameters
  • depth_image (NDArray[(typing.Any, typing.Any), float32]) – The depth image to process.

  • kernel_size (int) – The size of the area to average around each pixel.

Return type

Tuple[int, int]

Returns

The (row, column) of the pixel which is closest to the car.

Warning

kernel_size be positive and odd. It is highly recommended that you crop off the bottom of the image, or else this function will likely return the ground directly in front of the car.

Note

The larger the kernel_size, the more that the depth of each pixel is averaged with the distances of the surrounding pixels. This helps reduce noise at the cost of reduced accuracy.

Example:

depth_image = rc.camera.get_depth_image()

# Crop off the ground directly in front of the car
cropped_image = rc_utils.crop(
    image, (0, 0), (int(rc.camera.get_height() * 0.66), rc.camera.get_width())
)

# Find the closest pixel
closest_pixel = rc_utils.get_closest_pixel(depth_image)
racecar_utils.get_contour_area(contour)

Finds the area of a contour from an image.

Parameters

contour (NDArray) – The contour of which to measure the area.

Return type

float

Returns

The number of pixels contained within the contour

Example:

# Extract the largest blue contour
BLUE_HSV_MIN = (90, 50, 50)
BLUE_HSV_MAX = (110, 255, 255)
contours = rc_utils.find_contours(
    rc.camera.get_color_image(), BLUE_HSV_MIN, BLUE_HSV_MAX
)
largest_contour = rc_utils.get_largest_contour(contours)

# Find the area of this contour (will evaluate to 0 if no contour was found)
area = rc_utils.get_contour_area(contour)
racecar_utils.get_contour_center(contour)

Finds the center of a contour from an image.

Parameters

contour (NDArray) – The contour of which to find the center.

Return type

Optional[Tuple[int, int]]

Returns

The (row, column) of the pixel at the center of the contour, or None if the contour is empty.

Example:

# Extract the largest blue contour
BLUE_HSV_MIN = (90, 50, 50)
BLUE_HSV_MAX = (110, 255, 255)
contours = rc_utils.find_contours(
    rc.camera.get_color_image(), BLUE_HSV_MIN, BLUE_HSV_MAX
)
largest_contour = rc_utils.get_largest_contour(contours)

# Find the center of this contour if it exists
if (largest_contour is not None):
    center = rc_utils.get_contour_center(largest_contour)
racecar_utils.get_depth_image_center_distance(depth_image, kernel_size=5)

Finds the distance of the center object in a depth image.

Parameters
  • depth_image (NDArray[(typing.Any, typing.Any), float32]) – The depth image to process.

  • kernel_size (int) – The size of the area to average around the center.

Return type

float

Returns

The distance in cm of the object in the center of the image.

Warning

kernel_size must be positive and odd.

Note

The larger the kernel_size, the more that the center is averaged with the depth of the surrounding pixels. This helps reduce noise at the cost of reduced accuracy. If kernel_size = 1, no averaging is done.

Example:

depth_image = rc.camera.get_depth_image()

# Find the distance of the object (in cm) the center of depth_image
center_distance = rc_utils.get_depth_image_center_distance(depth_image)
racecar_utils.get_largest_contour(contours, min_area=30)

Finds the largest contour with size greater than min_area.

Parameters
  • contours (List[NDArray]) – A list of contours found in an image.

  • min_area (int) – The smallest contour to consider (in number of pixels)

Return type

Optional[NDArray]

Returns

The largest contour from the list, or None if no contour was larger than min_area.

Example:

# Extract the blue contours
BLUE_HSV_MIN = (90, 50, 50)
BLUE_HSV_MAX = (110, 255, 255)
contours = rc_utils.find_contours(
    rc.camera.get_color_image(), BLUE_HSV_MIN, BLUE_HSV_MAX
)

# Find the largest contour
largest_contour = rc_utils.get_largest_contour(contours)
racecar_utils.get_lidar_average_distance(scan, angle, window_angle=4)

Finds the average distance of the object at a particular angle relative to the car.

Parameters
  • scan (NDArray[Any, float32]) – The samples from a LIDAR scan

  • angle (float) – The angle (in degrees) at which to measure distance, starting at 0 directly in front of the car and increasing clockwise.

  • window_angle (float) – The number of degrees to consider around angle.

Return type

float

Returns

The average distance of the points at angle in cm.

Note

Ignores any samples with a value of 0.0 (no data). Increasing window_angle reduces noise at the cost of reduced accuracy.

Example:

scan = rc.lidar.get_samples()

# Find the distance directly behind the car (6:00 position)
back_distance = rc_utils.get_lidar_average_distance(scan, 180)

# Find the distance to the forward and right of the car (1:30 position)
forward_right_distance = rc_utils.get_lidar_average_distance(scan, 45)
racecar_utils.get_lidar_closest_point(scan, window=0, 360)

Finds the closest point from a LIDAR scan.

Parameters
  • scan (NDArray[Any, float32]) – The samples from a LIDAR scan.

  • window (Tuple[float, float]) – The degree range to consider, expressed as (min_degree, max_degree)

Return type

Tuple[float, float]

Returns

The (angle, distance) of the point closest to the car within the specified degree window. All angles are in degrees, starting at 0 directly in front of the car and increasing clockwise. Distance is in cm.

Warning

In areas with glass, mirrors, or large open spaces, there is a high likelihood of distance error.

Note

Ignores any samples with a value of 0.0 (no data).

In order to define a window which passes through the 360-0 degree boundary, it is acceptable for window min_degree to be larger than window max_degree. For example, (350, 10) is a 20 degree window in front of the car.

Example:

scan = rc.lidar.get_samples()

# Find the angle and distance of the closest point
angle, distance = rc_utils.get_lidar_closest_point(scan)

# Find the closest distance in the 90 degree window behind the car
_, back_distance = rc_utils.get_lidar_closest_point(scan, (135, 225))

# Find the closest distance in the 90 degree window in front of the car
_, front_distance = rc_utils.get_lidar_closest_point(scan, (315, 45))
racecar_utils.get_pixel_average_distance(depth_image, pix_coord, kernel_size=5)

Finds the distance of a pixel averaged with its neighbors in a depth image.

Parameters
  • depth_image (NDArray[(typing.Any, typing.Any), float32]) – The depth image to process.

  • pix_coord (Tuple[int, int]) – The (row, column) of the pixel to measure.

  • kernel_size (int) – The size of the area to average around the pixel.

Return type

float

Returns

The distance in cm of the object at the provided pixel.

Warning

kernel_size must be positive and odd.

Note

The larger the kernel_size, the more that the requested pixel is averaged with the distances of the surrounding pixels. This helps reduce noise at the cost of reduced accuracy.

Example:

depth_image = rc.camera.get_depth_image()

# Find the distance of the object (in cm) at the pixel (100, 20) of depth_image
average_distance = rc_utils.get_average_distance(depth_image, 100, 20)
racecar_utils.remap_range(val, old_min, old_max, new_min, new_max, saturate=False)

Remaps a value from one range to another range.

Parameters
  • val (float) – A number form the old range to be rescaled.

  • old_min (float) – The inclusive ‘lower’ bound of the old range.

  • old_max (float) – The inclusive ‘upper’ bound of the old range.

  • new_min (float) – The inclusive ‘lower’ bound of the new range.

  • new_max (float) – The inclusive ‘upper’ bound of the new range.

  • saturate (bool) – If true, the new_min and new_max limits are enforced.

Note

min need not be less than max; flipping the direction will cause the sign of the mapping to flip. val does not have to be between old_min and old_max.

Example:

# a will be set to 25
a = rc_utils.remap_range(5, 0, 10, 0, 50)

# b will be set to 975
b = rc_utils.remap_range(5, 0, 20, 1000, 900)

# c will be set to 30
c = rc_utils.remap_range(2, 0, 1, -10, 10)

# d will be set to 10
d = rc_utils.remap_range(2, 0, 1, -10, 10, True)
Return type

float

racecar_utils.stack_images_horizontal(image_0, image_1)

Stack two images horizontally.

Parameters
  • image_0 (NDArray[(typing.Any, Ellipsis), Any]) – The image to place on the left.

  • image_1 (NDArray[(typing.Any, Ellipsis), Any]) – The image to place on the right.

Return type

NDArray[(typing.Any, Ellipsis), Any]

Returns

An image with the original two images next to each other.

Note

The images must have the same height.

Example::

color_image = rc.camera.get_color_image()

depth_image = rc.camera.get_depth_image() depth_image_colormap = rc_utils.colormap_depth_image(depth_image)

# Create a new image with the color on the left and depth on the right new_image = rc_utils.stack_images_horizontally(color_image, depth_image_colormap)

racecar_utils.stack_images_vertical(image_0, image_1)

Stack two images vertically.

Parameters
  • image_0 (NDArray[(typing.Any, Ellipsis), Any]) – The image to place on the top.

  • image_1 (NDArray[(typing.Any, Ellipsis), Any]) – The image to place on the bottom.

Return type

NDArray[(typing.Any, Ellipsis), Any]

Returns

An image with the original two images on top of eachother.

Note

The images must have the same width.

Example::

color_image = rc.camera.get_color_image()

depth_image = rc.camera.get_depth_image() depth_image_colormap = rc_utils.colormap_depth_image(depth_image)

# Create a new image with the color on the top and depth on the bottom new_image = rc_utils.stack_images_vertically(color_image, depth_image_colormap)