def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
# Use the triggers to control the car's speed
rt = rc.controller.get_trigger(rc.controller.Trigger.RIGHT)
lt = rc.controller.get_trigger(rc.controller.Trigger.LEFT)
speed = rt - lt
# Calculate the distance in front of and behind the car
scan = rc.lidar.get_samples()
_, forward_dist = rc_utils.get_lidar_closest_point(scan, FRONT_WINDOW)
_, back_dist = rc_utils.get_lidar_closest_point(scan, REAR_WINDOW)
# TODO (warmup): Prevent the car from hitting things in front or behind it.
# Allow the user to override safety stop by holding the left or right bumper.
# Use the left joystick to control the angle of the front wheels
angle = rc.controller.get_joystick(rc.controller.Joystick.LEFT)[0]
rc.drive.set_speed_angle(speed, angle)
# Print the current speed and angle when the A button is held down
if rc.controller.is_down(rc.controller.Button.A):
print("Speed:", speed, "Angle:", angle)
# Print the distance of the closest object in front of and behind the car
if rc.controller.is_down(rc.controller.Button.B):
print("Forward distance:", forward_dist, "Back distance:", back_dist)
# Display the current LIDAR scan
rc.display.show_lidar(scan)
def start():
"""
This function is run once every time the start button is pressed
"""
global cur_speed
global prev_forward_dist
global prev_back_dist
# Have the car begin at a stop
rc.drive.stop()
# Initialize variables
cur_speed = 0
scan = rc.lidar.get_samples()
_, forward_dist = rc_utils.get_lidar_closest_point(scan, FRONT_WINDOW)
_, back_dist = rc_utils.get_lidar_closest_point(scan, REAR_WINDOW)
# Print start message
print(">> Lab 5A - LIDAR Safety Stop\n"
"\n"
"Controls:\n"
" Right trigger = accelerate forward\n"
" Right bumper = override forward safety stop\n"
" Left trigger = accelerate backward\n"
" Left bumper = override rear safety stop\n"
" Left joystick = turn front wheels\n"
" A button = print current speed and angle\n"
" B button = print forward and back distances")
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
scan = rc.lidar.get_samples()
highlighted_samples = [
] # Samples we will highlight in the LIDAR visualization
# Choose speed based on forward distance
_, front_dist = rc_utils.get_lidar_closest_point(
scan, (-FRONT_DIST_ANGLE, FRONT_DIST_ANGLE))
speed = rc_utils.remap_range(front_dist, 0, BRAKE_DISTANCE, 0, MAX_SPEED,
True)
# Measure distance to the left and right walls ahead of the car
left_ahead = rc_utils.get_lidar_average_distance(scan, -SIDE_ANGLE)
right_ahead = rc_utils.get_lidar_average_distance(scan, SIDE_ANGLE)
ratio = left_ahead / right_ahead if left_ahead > 0 and right_ahead > 0 else 1.0
# If there is a wall ahead and the left wall is significantly father, assume that
# the hallway has turned left
if front_dist < BRAKE_DISTANCE and ratio > LEFT_TURN_RATIO:
angle = -1
print("HARD LEFT: ratio", ratio, "front dist", front_dist)
# Otherwise, try to stay GOAL_DIST away from the right wall
else:
right_wall_angle, right_wall_dist = rc_utils.get_lidar_closest_point(
scan, WINDOW_ANGLES)
angle = rc_utils.remap_range(right_wall_dist, GOAL_DIST / 2, GOAL_DIST,
-1, 0)
angle = rc_utils.clamp(angle, -1, 1)
# Display the closest point on the right wall
highlighted_samples = [(right_wall_angle, right_wall_dist)]
rc.drive.set_speed_angle(speed, angle)
rc.display.show_lidar(scan, highlighted_samples=highlighted_samples)
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():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
global cur_speed
global prev_forward_dist
global prev_back_dist
# Use the triggers to control the car's speed
rt = rc.controller.get_trigger(rc.controller.Trigger.RIGHT)
lt = rc.controller.get_trigger(rc.controller.Trigger.LEFT)
speed = rt - lt
# Calculate the distance in front of and behind the car
scan = rc.lidar.get_samples()
_, forward_dist = rc_utils.get_lidar_closest_point(scan, FRONT_WINDOW)
_, back_dist = rc_utils.get_lidar_closest_point(scan, REAR_WINDOW)
frame_speed: float
if forward_dist < back_dist:
frame_speed = (prev_forward_dist - forward_dist) / rc.get_delta_time()
else:
frame_speed = -(prev_back_dist - back_dist) / rc.get_delta_time()
cur_speed += ALPHA * (frame_speed - cur_speed)
prev_forward_dist = forward_dist
prev_back_dist = back_dist
stop_distance: float
# FORARD SAFETY STOP: We are about to hit something in front of us
if not rc.controller.is_down(rc.controller.Button.RB) and cur_speed > 0:
# Calculate slow and stop distances based on the current speed
stop_distance = rc_utils.clamp(
MIN_STOP_DISTANCE +
cur_speed * abs(cur_speed) * STOP_DISTANCE_SCALE,
MIN_STOP_DISTANCE,
MAX_STOP_DISTANCE,
)
slow_distance = stop_distance * SLOW_DISTANCE_RATIO
# If we are past slow_distance, reduce speed proportional to how close we are
# to stop_distance
if stop_distance < forward_dist < slow_distance:
speed = min(
speed,
rc_utils.remap_range(forward_dist, stop_distance,
slow_distance, 0, 0.5),
)
print("Forward safety slow: speed limited to {}".format(speed))
# Safety stop if we are passed stop_distance by reversing at a speed
# proportional to how far we are past stop_distance
if 0 < forward_dist < stop_distance:
speed = rc_utils.remap_range(forward_dist, 0, stop_distance, -4,
-0.2)
speed = rc_utils.clamp(speed, -1, -0.2)
print("Forward safety stop: reversing at {}".format(speed))
# REAR SAFETY STOP: We are about to hit something behind us
if not rc.controller.is_down(rc.controller.Button.LB) and cur_speed < 0:
stop_distance = rc_utils.clamp(
MIN_STOP_DISTANCE -
cur_speed * abs(cur_speed) * STOP_DISTANCE_SCALE,
MIN_STOP_DISTANCE,
MAX_STOP_DISTANCE,
)
slow_distance = stop_distance * SLOW_DISTANCE_RATIO
if stop_distance < back_dist < slow_distance:
speed = max(
speed,
rc_utils.remap_range(back_dist, stop_distance, slow_distance,
0, -0.5),
)
print("Back safety slow: speed limited to {}".format(speed))
if 0 < back_dist < stop_distance:
speed = rc_utils.remap_range(back_dist, 0, stop_distance, 4, 0.2,
True)
speed = rc_utils.clamp(speed, 0.2, 1)
print("Back safety stop: accelerating forward at {}".format(speed))
# Use the left joystick to control the angle of the front wheels
angle = rc.controller.get_joystick(rc.controller.Joystick.LEFT)[0]
rc.drive.set_speed_angle(speed, angle)
# Print the current speed and angle when the A button is held down
if rc.controller.is_down(rc.controller.Button.A):
print("Speed:", speed, "Angle:", angle)
# Print the distance of the closest object in front of and behind the car
if rc.controller.is_down(rc.controller.Button.B):
print("Forward distance:", forward_dist, "Back distance:", back_dist)
# Print cur_speed estimate and stop distance when the X button is held down
if rc.controller.is_down(rc.controller.Button.X):
print("Current speed estimate: {:.2f} cm/s, Stop distance: {:.2f}".
format(cur_speed, stop_distance))
# Display the current LIDAR scan
rc.display.show_lidar(scan)
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():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
global cur_mode
scan = rc.lidar.get_samples()
speed = 0
angle = 0
# Find the minimum distance to the front, side, and rear of the car
front_angle, front_dist = rc_utils.get_lidar_closest_point(
scan, (-MIN_SIDE_ANGLE, MIN_SIDE_ANGLE)
)
left_angle, left_dist = rc_utils.get_lidar_closest_point(
scan, (-MAX_SIDE_ANGLE, -MIN_SIDE_ANGLE)
)
right_angle, right_dist = rc_utils.get_lidar_closest_point(
scan, (MIN_SIDE_ANGLE, MAX_SIDE_ANGLE)
)
# Estimate the left wall angle relative to the car by comparing the distance
# to the left-front and left-back
left_front_dist = rc_utils.get_lidar_average_distance(
scan, -SIDE_FRONT_ANGLE, WINDOW_ANGLE
)
left_back_dist = rc_utils.get_lidar_average_distance(
scan, -SIDE_BACK_ANGLE, WINDOW_ANGLE
)
left_dif = left_front_dist - left_back_dist
# Use the same process for the right wall angle
right_front_dist = rc_utils.get_lidar_average_distance(
scan, SIDE_FRONT_ANGLE, WINDOW_ANGLE
)
right_back_dist = rc_utils.get_lidar_average_distance(
scan, SIDE_BACK_ANGLE, WINDOW_ANGLE
)
right_dif = right_front_dist - right_back_dist
# If we are within PANIC_DISTANCE of either wall, enter panic mode
if left_dist < PANIC_DISTANCE or right_dist < PANIC_DISTANCE:
cur_mode = Mode.left_panic if left_dist < right_dist else Mode.right_panic
# If there are no visible walls to follow, stop the car
if left_front_dist == 0.0 and right_front_dist == 0.0:
speed = 0
angle = 0
# LEFT PANIC: We are close to hitting a wall to the left, so turn hard right
elif cur_mode == Mode.left_panic:
angle = 1
speed = PANIC_SPEED
if left_dist > END_PANIC_DISTANCE:
cur_mode = Mode.align
# RIGHT PANIC: We are close to hitting a wall to the right, so turn hard left
elif cur_mode == Mode.right_panic:
angle = -1
speed = PANIC_SPEED
if right_dist > END_PANIC_DISTANCE:
cur_mode = Mode.align
# ALIGN: Try to align straight and equidistant between the left and right walls
else:
# If left_dif is very large, the hallway likely turns to the left
if left_dif > TURN_THRESHOLD:
angle = -1
# Similarly, if right_dif is very large, the hallway likely turns to the right
elif right_dif > TURN_THRESHOLD:
angle = 1
# Otherwise, determine angle by taking into account both the relative angles and
# distances of the left and right walls
value = (right_dif - left_dif) + (right_dist - left_dist)
angle = rc_utils.remap_range(
value, -TURN_THRESHOLD, TURN_THRESHOLD, -1, 1, True
)
# Choose speed based on the distance of the object in front of the car
speed = rc_utils.remap_range(front_dist, 0, BRAKE_DISTANCE, 0, MAX_SPEED, True)
rc.drive.set_speed_angle(speed, angle)
# Show the lidar scan, highlighting the samples used as min_dist measurements
highlighted_samples = [
(front_angle, front_dist),
(left_angle, left_dist),
(right_angle, right_dist),
]
rc.display.show_lidar(scan, highlighted_samples=highlighted_samples)
# Print the current speed and angle when the A button is held down
if rc.controller.is_down(rc.controller.Button.A):
print("Speed:", speed, "Angle:", angle)
# Print calculated statistics when the B button is held down
if rc.controller.is_down(rc.controller.Button.B):
print(
"front_dist {:.2f}, left_dist {:.2f} cm, left_dif {:.2f} cm, right_dist {:.2f} cm, right_dif {:.2f} cm".format(
front_dist, left_dist, left_dif, right_dist, right_dif
)
)
# Print the current mode when the X button is held down
if rc.controller.is_down(rc.controller.Button.X):
print("Mode:", cur_mode)
def update_images(self, rc):
self.__color_image = rc.camera.get_color_image()
self.__depth_image = cv.GaussianBlur((rc.camera.get_depth_image() - 0.01) % 10000, (c.BLUR_KERNEL_SIZE, c.BLUR_KERNEL_SIZE), 0)
self.__closest_lidar_sample = rc_utils.get_lidar_closest_point((rc.lidar.get_samples() - 0.1) * 1000, (0, 360))
