def run_slow(self, rc):
largest = self.get_2_largest(self.cropped_img, self.slow_col)
if self.cur_state == self.State.SLOW:
speed = c.SPEED_SLOW
else:
speed = -1
# If the fast lane is visible more than the slow one, then just keep turning in the previously set direction
largest_fast_cropped = rc_utils.get_largest_contour(
rc_utils.find_contours(self.cropped_img, self.fast_col.value[0],
self.fast_col.value[1]))
largest_slow_cropped = rc_utils.get_largest_contour(
rc_utils.find_contours(self.cropped_img, self.slow_col.value[0],
self.slow_col.value[1]))
slow_a = rc_utils.get_contour_area(
largest_slow_cropped) if largest_slow_cropped is not None else 0
fast_a = rc_utils.get_contour_area(
largest_fast_cropped) if largest_fast_cropped is not None else 0
if len(largest) == 2 and not slow_a < fast_a:
self.angle = 0
elif len(largest) == 1 and not slow_a < fast_a:
cont = largest[0][1]
# Finds the top point of the contour and the bottom (Estimates line slope)
top_pt = tuple([
pt[0] for pt in cont if pt[0][1] == np.amin(cont[:, :, 1])
][0])
bott_pt = tuple([
pt[0] for pt in cont if pt[0][1] == np.amax(cont[:, :, 1])
][0])
# Slop is sloppy?????????????
if self.slow_state_angle == 0:
if top_pt[0] - bott_pt[0] > 0:
self.slow_state_angle = 1
elif top_pt[0] - bott_pt[0] < 0:
self.slow_state_angle = -1
else:
self.slow_state_angle = 0
self.angle = self.slow_state_angle
# Draws VIZHUALS
#cv.line(self.cropped_img, top_pt, bott_pt, (255, 255, 0), thickness=2)
#rc.display.show_color_image(self.cropped_img)
# Sets speed and angle
rc.drive.set_speed_angle(rc_utils.clamp(speed, -1, 1),
rc_utils.clamp(self.angle, -1, 1))
return len(largest)
def update_contour():
"""
Finds contours in the current color image and uses them to update contour_center
and contour_area
"""
global contour_center
global contour_area
image = rc.camera.get_color_image()
if image is None:
contour_center = None
contour_area = 0
else:
# Find all of the orange contours
contours = rc_utils.find_contours(image, ORANGE[0], ORANGE[1])
# Select the largest contour
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(contour)
contour_area = rc_utils.get_contour_area(contour)
# Draw contour onto the image
rc_utils.draw_contour(image, contour)
rc_utils.draw_circle(image, contour_center)
else:
contour_center = None
contour_area = 0
# Display the image to the screen
rc.display.show_color_image(image)
def updateContour(self, rc, depth_image, color_image):
if color_image is None:
self.contour_center = None
else:
# Crop the image to the floor directly in front of the car
contour_image = rc_utils.crop(color_image, c.LINE_CROP_FLOOR[0],
c.LINE_CROP_FLOOR[1])
contours = rc_utils.find_contours(contour_image,
self.color.value[0],
self.color.value[1])
L_contour = rc_utils.get_largest_contour(contours,
c.LINE_MIN_CONTOUR_AREA)
if L_contour is not None:
self.contour_center = rc_utils.get_contour_center(L_contour)
contour_area = rc_utils.get_contour_area(L_contour)
# Draw contour onto the image
rc_utils.draw_contour(contour_image, L_contour, (0, 255, 255))
rc_utils.draw_circle(contour_image, self.contour_center,
(0, 255, 255))
def get_closest_contour(contours, depth_img):
MIN_CONTOUR_AREA = 70
MAX_CONTOUR_DIST = 300
# Gets centers of each contour and extracts countours based on conditions
centers = []
for idx, contour in enumerate(contours):
cent = rc_utils.get_contour_center(contour)
if cent is not None:
dist = rc_utils.get_pixel_average_distance(depth_img, cent)
area = rc_utils.get_contour_area(contour)
if area > MIN_CONTOUR_AREA and dist < MAX_CONTOUR_DIST:
centers.append((idx, rc_utils.get_contour_center(contour)))
indexes = [center[0] for center in centers]
centers = [center[1] for center in centers]
# Calculates the distance to each center
distances = [rc_utils.get_pixel_average_distance(depth_img, (center[0], center[1])) for center in centers]
conts = [contours[index] for index in indexes]
# Finds smallest distance and index of that distance
if len(conts):
minimum = min(enumerate(distances), key=lambda x: x[1])
# (index, min dist)
# Returns distance to closest contour center, the contour itself, and the center position
return (minimum[1], conts[minimum[0]], centers[minimum[0]])
else:
# If there is no contour, my love for humanities is returned
return None
def get_2_largest(self, img, col):
# Gets all fast contours and sorts them by contour area
contours = list(
enumerate(rc_utils.find_contours(img, col.value[0], col.value[1])))
areas = [rc_utils.get_contour_area(cont[1]) for cont in contours]
contours = sorted(contours, key=lambda x: areas[x[0]], reverse=True)
# Gets 2 largest contours
largest = contours[:2]
largest = [
cont for cont in largest
if areas[cont[0]] > c.LANE_MIN_CONTOUR_AREA
]
return largest
def update_contour():
"""
Finds contours in the current color image and uses them to update contour_center
and contour_area
"""
global contour_center
global contour_area
image = rc.camera.get_color_image()
if image is None:
contour_center = None
contour_area = 0
else:
# TODO (challenge 1): Search for multiple tape colors with a priority order DONE
# (currently we only search for blue)
# Crop the image to the floor directly in front of the car
image = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1])
# Find all of the colored contours
for color in color_priority:
contours = rc_utils.find_contours(image, color[0], color[1])
if len(contours) > 0:
break
# Select the largest contour
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(contour)
contour_area = rc_utils.get_contour_area(contour)
# Draw contour onto the image
rc_utils.draw_contour(image, contour)
rc_utils.draw_circle(image, contour_center)
else:
contour_center = None
contour_area = 0
# Display the image to the screen
rc.display.show_color_image(image)
def update_contour():
"""
Finds contours in the current color image and uses them to update contour_center
and contour_area
"""
global contour_center
global contour_area
image = rc.camera.get_color_image()
if image is None:
contour_center = None
contour_area = 0
else:
# Crop the image to the floor directly in front of the car
image = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1])
# Search for each color in priority order
for color in COLOR_PRIORITY:
# Find all of the contours of the current color
contours = rc_utils.find_contours(image, color[0], color[1])
# Select the largest contour
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(contour)
contour_area = rc_utils.get_contour_area(contour)
# Draw contour onto the image
rc_utils.draw_contour(image, contour)
rc_utils.draw_circle(image, contour_center)
break
# If no contours are found for any color, set center and area accordingly
else:
contour_center = None
contour_area = 0
# Display the image to the screen
rc.display.show_color_image(image)
def updateContour(color_priority):
image = rc.camera.get_color_image()
if image is None:
contour_center = None
contour_area = 0
else:
image_cropped = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1])
for color in color_priority:
contours = rc_utils.find_contours(image_cropped,color[0],color[1])
if len(contours) > 0:
break
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
contour_center = rc_utils.get_contour_center(contour)
contour_area = rc_utils.get_contour_area(contour)
else:
contour_center = None
contour_area = 0
rc.display.show_color_image(image)
return contour_center,contour_area
def get_cont_area_proofed(contour):
if contour is None or not len(contour):
return 0
else:
return rc_utils.get_contour_area(contour)
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
# Display the color image cropped to the top left
if rc.controller.was_pressed(rc.controller.Button.A):
image = rc.camera.get_color_image()
cropped = rc_utils.crop(
image, (0, 0),
(rc.camera.get_height() // 2, rc.camera.get_width() // 2))
rc.display.show_color_image(cropped)
# Find and display the largest red contour in the color image
if rc.controller.was_pressed(rc.controller.Button.B):
image = rc.camera.get_color_image()
contours = rc_utils.find_contours(image, RED[0], RED[1])
largest_contour = rc_utils.get_largest_contour(contours)
if largest_contour is not None:
center = rc_utils.get_contour_center(largest_contour)
area = rc_utils.get_contour_area(largest_contour)
print("Largest red contour: center={}, area={:.2f}".format(
center, area))
rc_utils.draw_contour(image, largest_contour,
rc_utils.ColorBGR.green.value)
rc_utils.draw_circle(image, center, rc_utils.ColorBGR.yellow.value)
rc.display.show_color_image(image)
else:
print("No red contours found")
# Print depth image statistics and show the cropped upper half
if rc.controller.was_pressed(rc.controller.Button.X):
depth_image = rc.camera.get_depth_image()
# Measure average distance at several points
left_distance = rc_utils.get_pixel_average_distance(
depth_image,
(rc.camera.get_height() // 2, rc.camera.get_width() // 4),
)
center_distance = rc_utils.get_depth_image_center_distance(depth_image)
center_distance_raw = rc_utils.get_depth_image_center_distance(
depth_image, 1)
right_distance = rc_utils.get_pixel_average_distance(
depth_image,
(rc.camera.get_height() // 2, 3 * rc.camera.get_width() // 4),
)
print(f"Depth image left distance: {left_distance:.2f} cm")
print(f"Depth image center distance: {center_distance:.2f} cm")
print(f"Depth image raw center distance: {center_distance_raw:.2f} cm")
print(f"Depth image right distance: {right_distance:.2f} cm")
# Measure pixels where the kernel falls off the edge of the photo
upper_left_distance = rc_utils.get_pixel_average_distance(
depth_image, (2, 1), 11)
lower_right_distance = rc_utils.get_pixel_average_distance(
depth_image,
(rc.camera.get_height() - 2, rc.camera.get_width() - 5), 13)
print(f"Depth image upper left distance: {upper_left_distance:.2f} cm")
print(
f"Depth image lower right distance: {lower_right_distance:.2f} cm")
# Find closest point in bottom third
cropped = rc_utils.crop(
depth_image,
(0, 0),
(rc.camera.get_height() * 2 // 3, rc.camera.get_width()),
)
closest_point = rc_utils.get_closest_pixel(cropped)
closest_distance = cropped[closest_point[0]][closest_point[1]]
print(
f"Depth image closest point (upper half): (row={closest_point[0]}, col={closest_point[1]}), distance={closest_distance:.2f} cm"
)
rc.display.show_depth_image(cropped, points=[closest_point])
# Print lidar statistics and show visualization with closest point highlighted
if rc.controller.was_pressed(rc.controller.Button.Y):
lidar = rc.lidar.get_samples()
front_distance = rc_utils.get_lidar_average_distance(lidar, 0)
right_distance = rc_utils.get_lidar_average_distance(lidar, 90)
back_distance = rc_utils.get_lidar_average_distance(lidar, 180)
left_distance = rc_utils.get_lidar_average_distance(lidar, 270)
print(f"Front LIDAR distance: {front_distance:.2f} cm")
print(f"Right LIDAR distance: {right_distance:.2f} cm")
print(f"Back LIDAR distance: {back_distance:.2f} cm")
print(f"Left LIDAR distance: {left_distance:.2f} cm")
closest_sample = rc_utils.get_lidar_closest_point(lidar)
print(
f"Closest LIDAR point: {closest_sample[0]:.2f} degrees, {closest_sample[1]:.2f} cm"
)
rc.display.show_lidar(lidar, highlighted_samples=[closest_sample])
# Print lidar distance in the direction the right joystick is pointed
rjoy_x, rjoy_y = rc.controller.get_joystick(rc.controller.Joystick.RIGHT)
if abs(rjoy_x) > 0 or abs(rjoy_y) > 0:
lidar = rc.lidar.get_samples()
angle = (math.atan2(rjoy_x, rjoy_y) * 180 / math.pi) % 360
distance = rc_utils.get_lidar_average_distance(lidar, angle)
print(f"LIDAR distance at angle {angle:.2f} = {distance:.2f} cm")
# Default drive-style controls
left_trigger = rc.controller.get_trigger(rc.controller.Trigger.LEFT)
right_trigger = rc.controller.get_trigger(rc.controller.Trigger.RIGHT)
left_joystick = rc.controller.get_joystick(rc.controller.Joystick.LEFT)
rc.drive.set_speed_angle(right_trigger - left_trigger, left_joystick[0])
def update_contour():
"""
Finds contours in the current color image and uses them to update contour_center
and contour_area
"""
global contour_center
global contour_area
global cur_state
global FIRST_PRI1
global SECOND_PRI1
#global THIRD_PRI
global red_dir
global blue_dir
global green_dir
contour_image = rc.camera.get_color_image()
if contour_image is None:
contour_center = None
contour_area = 0
else:
# TODO (challenge 1): Search for multiple tape colors with a priority order
# (currently we only search for blue)
# Crop the image to the floor directly in front of the car
contour_image = rc_utils.crop(contour_image, CROP_FLOOR[0],
CROP_FLOOR[1])
#Find all of the red contours
contours_red = rc_utils.find_contours(contour_image, RED[0], RED[1])
# Find all of the blue contours
contours_blue = rc_utils.find_contours(contour_image, BLUE[0], BLUE[1])
#Find all of the green contours
contours_green = rc_utils.find_contours(contour_image, GREEN[0],
GREEN[1])
# Select the largest contour
L_contour_blue = rc_utils.get_largest_contour(contours_blue,
MIN_CONTOUR_AREA)
L_contour_red = rc_utils.get_largest_contour(contours_red,
MIN_CONTOUR_AREA)
L_contour_green = rc_utils.get_largest_contour(contours_green,
MIN_CONTOUR_AREA)
# Priorities#####################################################################
if FIRST_PRI1:
if FIRST_PRI1 == red_dir:
FIRST_PRI = L_contour_red
if SECOND_PRI1 == blue_dir:
SECOND_PRI = L_contour_blue
THIRD_PRI = L_contour_green
else:
SECOND_PRI = L_contour_green
THIRD_PRI = L_contour_blue
elif FIRST_PRI1 == blue_dir:
FIRST_PRI = L_contour_blue
if SECOND_PRI1 == green_dir:
SECOND_PRI = L_contour_green
THIRD_PRI = L_contour_red
else:
SECOND_PRI = L_contour_red
THIRD_PRI = L_contour_green
elif FIRST_PRI1 == green_dir:
FIRST_PRI = L_contour_green
if SECOND_PRI1 == blue_dir:
SECOND_PRI = L_contour_blue
THIRD_PRI = L_contour_red
else:
SECOND_PRI = L_contour_red
THIRD_PRI = L_contour_blue
if FIRST_PRI is not None: # and contour_center_first<200:
# Calculate contour information
contour_center = rc_utils.get_contour_center(FIRST_PRI)
contour_area = rc_utils.get_contour_area(FIRST_PRI)
# Draw contour onto the image
rc_utils.draw_contour(contour_image, FIRST_PRI, (0, 255, 0))
rc_utils.draw_circle(contour_image, contour_center)
elif SECOND_PRI is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(SECOND_PRI)
contour_area = rc_utils.get_contour_area(SECOND_PRI)
# Draw contour onto the image
rc_utils.draw_contour(contour_image, SECOND_PRI, (0, 0, 255))
rc_utils.draw_circle(contour_image, contour_center)
elif THIRD_PRI is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(THIRD_PRI)
contour_area = rc_utils.get_contour_area(THIRD_PRI)
# Draw contour onto the image
rc_utils.draw_contour(contour_image, THIRD_PRI, (255, 0, 0))
rc_utils.draw_circle(contour_image, contour_center)
else:
contour_center = None
contour_area = 0