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 show_depth_image(
self,
image: NDArray[(Any, Any), np.float32],
max_depth: int = 1000,
points: List[Tuple[int, int]] = [],
) -> None:
"""
Displays a depth image in grayscale in a window.
Args:
image: The depth image to display to the screen.
max_depth: The farthest depth to show in the image in cm. Anything past
this depth is shown as black.
points: A list of points in (pixel row, pixel column) format to show on
the image as colored dots.
Example::
depth_image = rc.camera.get_depth_image()
# Show the depth_image captured by the camera.
rc.display.show_depth_image(depth_image)
# Show anything that is at most 500 cm away, and show a black cross at
# row 3, column 5
rc.display.show_depth_image(depth_image, 500, [(3, 5)])
"""
if self.__isHeadless:
return
assert max_depth > 0, "max_depth must be positive."
for point in points:
assert (
0 <= point[0] < image.shape[0]
and 0 <= point[1] < image.shape[1]
), f"The point [{point}] is not a valid pixel row and column within image."
color_image = rc_utils.colormap_depth_image(image, max_depth)
# Draw a dot at each point in points
for point in points:
rc_utils.draw_circle(
color_image,
point,
rc_utils.ColorBGR.green.value,
radius=self.__BIG_DOT_RADIUS,
)
rc_utils.draw_circle(
color_image,
point,
rc_utils.ColorBGR.blue.value,
radius=self.__SMALL_DOT_RADIUS,
)
self.show_color_image(color_image)
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
# TODO: Park the car 30 cm away from the closest orange cone.
# Use both color and depth information to handle cones of multiple sizes.
# You may wish to copy some of your code from lab2b.py
image = rc.camera.get_color_image()
contours = rc_utils.find_contours(image, ORANGE[0], ORANGE[1])
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
contour_center = rc_utils.get_contour_center(contour)
rc_utils.draw_contour(image, contour)
rc_utils.draw_circle(image, contour_center)
else:
contour_center = 0
scan = rc.lidar.get_samples()
__, coneDist = rc_utils.get_lidar_closest_point(scan, (-20, 20))
if contour is not None:
global pastTerm
global derivTerm
angleTerm = contour_center[1] - rc.camera.get_width() / 2
speedTerm = coneDist - 50
derivTerm = (speedTerm - pastTerm) / rc.get_delta_time(
) if speedTerm != pastTerm else derivTerm
speedSign = speedTerm / abs(speedTerm) if speedTerm != 0 else 0
print(str(speedTerm) + " and " + str(derivTerm))
angle = angleTerm * (1 / 200) #angle P controller
speed = speedTerm * (1 / 50) + derivTerm * (1 / 250
) #speed P"D" controller
angle = -1 if angle < -1 else angle
angle = 1 if angle > 1 else angle
speed = -1 if speed < -1 else speed
speed = 1 if speed > 1 else speed
#speed = 0 if abs(derivTerm) < 15 else speed
else:
speed = 0
angle = 0
pastTerm = speedTerm
rc.drive.set_speed_angle(speed, angle)
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 update_contours(self, rc):
contours = rc_utils.find_contours(self.__color_image, self.COLORS[self.__cur_col.name][0], self.COLORS[self.__cur_col.name][1])
#get contours for color, find closest one
out = P_Slalom.get_closest_contour(contours, self.__depth_image)
if out is not None:
# Saves previous measured distance
#if self.__contour_distance is not None:
#self.__prev_distance = self.__contour_distance
self.__prev_distance = self.__contour_distance
self.__contour_distance, self.__contour, self.__contour_center = out
# Draws closest contour
rc_utils.draw_contour(self.__color_image, self.__contour)
rc_utils.draw_circle(self.__color_image, self.__contour_center)
else:
self.__contour_distance = None
self.__contour_center = None
self.__contour = None
def show_lidar(
self,
samples: NDArray[Any, np.float32],
radius: int = 128,
max_range: int = 1000,
highlighted_samples: List[Tuple[float, float]] = [],
) -> None:
"""
Displays a set of LIDAR samples.
Args:
samples: A complete LIDAR scan.
radius: Half of the width or height (in pixels) of the generated image.
max_range: The farthest depth to show in the image in cm. Anything past
this depth is shown as black.
highlighted_samples: A list of samples in (angle, distance) format to show
as light blue dots. Angle must be in degrees from straight ahead
(clockwise), and distance must be in cm.
Note:
Each sample in samples is shown as a red pixel. Each sample in
highlighted_samples is shown as a blue pixel. The car is shown as a green
dot at the center of the visualization.
Warning:
samples must be a complete LIDAR scan. This function assumes that each
sample is equal angle appart, and that samples spans the entire 360 degrees.
If this is not the case, the visualization will be inaccurate.
Example::
depth_image = rc.camera.get_depth_image()
# Show the depth_image captured by the camera.
rc.display.show_depth_image(depth_image)
# Show anything that is at most 500 cm away, and show a black cross at
# row 3, column 5
rc.display.show_depth_image(depth_image, 500, [(3, 5)])
"""
assert radius > 0, "radius must be positive."
assert max_range > 0, "max_range must be positive."
if self.__isHeadless:
return
# Create a square black image with the requested radius
image = np.zeros((2 * radius, 2 * radius, 3), np.uint8, "C")
num_samples: int = len(samples)
# Draw a red pixel for each non-zero sample less than max_range
for i in range(num_samples):
if 0 < samples[i] < max_range:
angle: float = 2 * math.pi * i / num_samples
length: float = radius * samples[i] / max_range
r: int = int(radius - length * math.cos(angle))
c: int = int(radius + length * math.sin(angle))
image[r][c][2] = 255
# Draw a green dot to denote the car
rc_utils.draw_circle(
image,
(radius, radius),
rc_utils.ColorBGR.green.value,
self.__LIDAR_CAR_RADIUS,
)
# Draw a light blue pixel for each point in highlighted_samples
for (angle, distance) in highlighted_samples:
if 0 < distance < max_range:
angle_rad = angle * math.pi / 180
length: float = radius * distance / max_range
r: int = int(radius - length * math.cos(angle_rad))
c: int = int(radius + length * math.sin(angle_rad))
image[r][c][0] = 255
image[r][c][1] = 255
image[r][c][2] = 0
self.show_color_image(image)
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 find_cones():
"""
Find the closest red and blue cones and update corresponding global variables.
"""
global red_center
global red_distance
global prev_red_distance
global blue_center
global blue_distance
global prev_blue_distance
prev_red_distance = red_distance
prev_blue_distance = blue_distance
color_image = rc.camera.get_color_image()
depth_image = rc.camera.get_depth_image()
if color_image is None or depth_image is None:
red_center = None
red_distance = 0
blue_center = None
blue_distance = 0
print("No image found")
return
# Search for the red cone
contours = rc_utils.find_contours(color_image, RED[0], RED[1])
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
red_center = rc_utils.get_contour_center(contour)
red_distance = rc_utils.get_pixel_average_distance(depth_image, red_center)
# Only use count it if the cone is less than MAX_DISTANCE away
if red_distance <= MAX_DISTANCE:
rc_utils.draw_contour(color_image, contour, rc_utils.ColorBGR.green.value)
rc_utils.draw_circle(color_image, red_center, rc_utils.ColorBGR.green.value)
else:
red_center = None
red_distance = 0
else:
red_center = None
red_distance = 0
# Search for the blue cone
contours = rc_utils.find_contours(color_image, BLUE[0], BLUE[1])
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
blue_center = rc_utils.get_contour_center(contour)
blue_distance = rc_utils.get_pixel_average_distance(depth_image, blue_center)
# Only use count it if the cone is less than MAX_DISTANCE away
if blue_distance <= MAX_DISTANCE:
rc_utils.draw_contour(color_image, contour, rc_utils.ColorBGR.yellow.value)
rc_utils.draw_circle(
color_image, blue_center, rc_utils.ColorBGR.yellow.value
)
else:
blue_center = None
blue_distance = 0
else:
blue_center = None
blue_distance = 0
rc.display.show_color_image(color_image)
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
global speed
global angle
global cur_state
global PRIORITY
global prevangle
global cones_done
global cur_mode
global counter
# Get all images
image = rc.camera.get_color_image()
#cur_state == State.cone_slaloming
corners, ids = rc_utils.get_ar_markers(image)
length = len(corners)
if length > 0:
id = 300
index = 0
for idx in range(0, len(ids)):
if ids[idx] < id:
id = ids[idx]
index = idx
TL = corners[index][0][0]
TR = corners[index][0][1]
BL = corners[index][0][3]
area = (abs(TL[0] - TR[0]) +
abs(TL[1] - TR[1])) * (abs(TL[0] - BL[0]) + abs(TL[1] - BL[1]))
print(id[0], area)
if id[0] == 32 and area > 1900:
if cur_state is not State.cone_slaloming:
cur_mode = Mode.no_cones
counter = 0
cur_state = State.cone_slaloming
print("State: ", cur_state)
elif id[0] == 236 and area > 850:
cur_state = State.wall_parking
print("State: ", cur_state)
depth_image = rc.camera.get_depth_image()
###### Line Following State ######
if cur_state == State.line_following:
if image is None:
contour_center = None
else:
# Crop the image to the floor directly in front of the car
image = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1])
colorContours = []
contour = None
colorContours = []
red = checkRed(image)
green = checkGreen(image)
#blue = checkBlue(image)
yellow = checkYellow(image)
for priority in PRIORITY:
if priority == "Y" and yellow is not None:
colorContours.append(yellow)
print("yellow")
elif priority == "R" and red is not None:
colorContours.append(red)
print("red")
elif priority == "G" and green is not None:
colorContours.append(green)
print("green")
if not colorContours:
angle = prevangle
contour = None
else:
contour = colorContours[0]
if contour is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(contour)
# Draw contour onto the image
rc_utils.draw_contour(image, contour)
rc_utils.draw_circle(image, contour_center)
#change
else:
contour_center = None
if contour_center is not None:
angle = rc_utils.remap_range(contour_center[1], 0,
rc.camera.get_width(), -1, 1,
True)
angle = rc_utils.clamp(angle, -1, 1)
prevangle = angle
# Display the image to the screen
rc.display.show_color_image(image)
##### Cone Slaloming State ######
elif cur_state == State.cone_slaloming:
print("cone slaloming")
update_cones()
###### Wall Parking State ######
elif cur_state == State.wall_parking:
print("Wall Parking")
# Get distance at 1/4, 2/4, and 3/4 width
center_dist = rc_utils.get_depth_image_center_distance(depth_image)
left_dist = rc_utils.get_pixel_average_distance(
depth_image, LEFT_POINT, KERNEL_SIZE)
right_dist = rc_utils.get_pixel_average_distance(
depth_image, RIGHT_POINT, KERNEL_SIZE)
print("distance", center_dist)
# Get difference between left and right distances
dist_dif = left_dist - right_dist
print("dist_dif", dist_dif)
# Remap angle
angle = rc_utils.remap_range(dist_dif, -MAX_DIST_DIF, MAX_DIST_DIF, -1,
1, True)
if abs(dist_dif) > 1:
print("entered")
angle = rc_utils.remap_range(dist_dif, -MAX_DIST_DIF, MAX_DIST_DIF,
-1, 1, True)
if center_dist > 20:
speed = 0.5
elif center_dist < 21 and center_dist > 10:
speed = rc_utils.remap_range(center_dist, 20, 10, 0.5, 0)
speed = rc_utils.clamp(speed, 0, 0.5)
else:
speed = 0
print("speed", speed)
rc.drive.set_speed_angle(speed, angle)
else:
# stop moving
rc.drive.stop()
print("angle", angle)
print("speed", speed)
rc.drive.set_speed_angle(0.6, angle)
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
