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 """ # 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 run_phase(self, rc, depth_image, color_image, lidar_scan): #print(">> Running Wall Following") """ 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 left joystick for angle #rt = rc.controller.get_trigger(rc.controller.Trigger.RIGHT) #lt = rc.controller.get_trigger(rc.controller.Trigger.LEFT) #speed = rt - lt #angle = rc.controller.get_joystick(rc.controller.Joystick.LEFT)[0] # Calculate the distance scan = lidar_scan max_wall = 0.65 #testing max speed hall = rc.camera.get_width() // 9 optimum = 60 rightDist = rc_utils.get_lidar_average_distance(scan, 44, 10) leftDist = rc_utils.get_lidar_average_distance(scan, 316, 10) angle = rc_utils.remap_range(rightDist - leftDist, -hall, hall, -1, 1) angle = rc_utils.clamp(angle, -1, 1) # get them tags corners, ids = rc_utils.get_ar_markers(color_image) billy = 150 if c.ar_in_range_ID(billy, depth_image, color_image, rc, 4 / 5) == c.ID_DIRECTION: dirrrrrrrrr = rc_utils.get_ar_direction(corners[0]) #print(dirrrrrrrrr) if dirrrrrrrrr == rc_utils.Direction.LEFT: angle = -1 elif dirrrrrrrrr == rc_utils.Direction.RIGHT: angle = 1 self.ar_tag = True elif self.is_canyon: tooClose = 80 if rc_utils.get_depth_image_center_distance( depth_image) < tooClose: angle = 1 right_farthest = np.amax( depth_image[rc.camera.get_height() * 5 // 6, rc.camera.get_width() // 2:rc.camera.get_width()].flatten()) left_farthest = np.amax( depth_image[rc.camera.get_height() * 5 // 6, 0:rc.camera.get_width() // 2].flatten()) diff = abs(right_farthest - left_farthest) AAAAAAAAAH_WE_ARE_ABOUT_TO_FALL____BETTER_STOP_NOW = 100 if self.ar_tag and self.ledge_count == 0 and diff > 50: if right_farthest > AAAAAAAAAH_WE_ARE_ABOUT_TO_FALL____BETTER_STOP_NOW: self.many += 1 self.ledge_angle = -1 self.ledge_count = 10 elif left_farthest > AAAAAAAAAH_WE_ARE_ABOUT_TO_FALL____BETTER_STOP_NOW: self.many += 1 self.ledge_angle = 1 self.ledge_count = 10 #print("left ", left_farthest, " right ", right_farthest) speed = rc_utils.remap_range(abs(angle), 15, 1, 1, 0.5) #temp controls if self.many == 3: self.ar_tag = False if self.ledge_count > 0: angle = self.ledge_angle self.ledge_count -= 1 rc.drive.set_speed_angle(max_wall * speed, angle)
def update(): """ After start() is run, this function is run every frame until the back button is pressed """ global max_speed global update_slow_time global show_triggers global show_joysticks # Check if each button was_pressed or was_released for button in rc.controller.Button: if rc.controller.was_pressed(button): print("Button {} was pressed".format(button.name)) if rc.controller.was_released(button): print("Button {} was released".format(button.name)) # Click left and right joystick to toggle showing trigger and joystick values 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) right_joystick = rc.controller.get_joystick(rc.controller.Joystick.RIGHT) if rc.controller.was_pressed(rc.controller.Button.LJOY): show_triggers = not show_triggers if rc.controller.was_pressed(rc.controller.Button.RJOY): show_joysticks = not show_joysticks if show_triggers: print("Left trigger: {}; Right trigger: {}".format( left_trigger, right_trigger)) if show_joysticks: print("Left joystick: {}; Right joystick: {}".format( left_joystick, right_joystick)) # Use triggers and left joystick to control car (like default drive) rc.drive.set_speed_angle(right_trigger - left_trigger, left_joystick[0]) # Change max speed and update_slow time when the bumper is pressed if rc.controller.was_pressed(rc.controller.Button.LB): max_speed = max(1 / 16, max_speed / 2) rc.drive.set_max_speed(max_speed) update_slow_time *= 2 rc.set_update_slow_time(update_slow_time) print("max_speed set to {}".format(max_speed)) print("update_slow_time set to {} seconds".format(update_slow_time)) if rc.controller.was_pressed(rc.controller.Button.RB): max_speed = min(1, max_speed * 2) rc.drive.set_max_speed(max_speed) update_slow_time /= 2 rc.set_update_slow_time(update_slow_time) print("max_speed set to {}".format(max_speed)) print("update_slow_time set to {} seconds".format(update_slow_time)) # Capture and display color images when the A button is down if rc.controller.is_down(rc.controller.Button.A): rc.display.show_color_image(rc.camera.get_color_image()) # Capture and display depth images when the B button is down elif rc.controller.is_down(rc.controller.Button.B): depth_image = rc.camera.get_depth_image() rc.display.show_depth_image(depth_image) print("Depth center distance: {:.2f} cm".format( rc_utils.get_depth_image_center_distance(depth_image))) # Capture and display Lidar data when the X button is down elif rc.controller.is_down(rc.controller.Button.X): lidar = rc.lidar.get_samples() rc.display.show_lidar(lidar) print("LIDAR forward distance: {:.2f} cm".format( rc_utils.get_lidar_average_distance(lidar, 0))) # Show IMU data when the Y button is pressed if rc.controller.is_down(rc.controller.Button.Y): a = rc.physics.get_linear_acceleration() w = rc.physics.get_angular_velocity() print("Linear acceleration: ({:5.2f},{:5.2f},{:5.2f}); ".format( a[0], a[1], a[2]) + "Angular velocity: ({:5.2f},{:5.2f},{:5.2f})".format( w[0], w[1], w[2]))
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)