Ejemplo n.º 1
0
    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))
Ejemplo n.º 2
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

    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)
Ejemplo n.º 3
0
    def run_fast(self, rc, dist_slow):
        # Get 2 largest fast contours
        largest = self.get_2_largest(self.cropped_img, self.fast_col)

        if len(largest) == 2:
            # Finds which contour is which (left side/ right side)
            avg_1 = np.average(largest[0][1][:, :, 0])
            avg_2 = np.average(largest[1][1][:, :, 0])

            avg_mid = (avg_2 + avg_1) / 2

            if avg_2 > avg_1:
                right_cont = largest[1][1]
                left_cont = largest[0][1]

            else:
                right_cont = largest[0][1]
                left_cont = largest[1][1]

            # Sets angle based on mid average offset from center
            self.angle = rc_utils.remap_range(
                avg_mid - (rc.camera.get_width() / 2), *c.ALIGN_REMAP)

            # <>>>>>>  VISZHUALS
            rc_utils.draw_contour(self.cropped_img,
                                  left_cont,
                                  color=(255, 0, 0))
            rc_utils.draw_contour(self.cropped_img,
                                  right_cont,
                                  color=(0, 255, 0))

            cv.line(self.cropped_img, (int(avg_mid), 0),
                    (int(avg_mid), rc.camera.get_height()), (255, 255, 0))

        elif len(largest) == 1:
            # If the center of the contour is roughly in the center of the screen, tell car to turn
            offset = abs(
                rc_utils.get_contour_center(largest[0][1])[1] -
                (rc.camera.get_width() // 2))

            if offset < c.LANE_SPLIT_DETECT_MAX_OFFSET:
                self.angle = 1

        # Sets speed based on distance from sharp turn and based on angle
        speed_approach_remap = rc_utils.remap_range(dist_slow,
                                                    *c.SLOWDOWN_REMAP)
        speed_angle_remap = rc_utils.remap_range(abs(self.angle),
                                                 *c.SPEED_ANGLE_REMAP)
        speed = min((speed_angle_remap, speed_approach_remap))

        # Checks ramp if y accel is below a threshold
        if rc.physics.get_linear_acceleration()[1] >= -9.59:
            speed = 1

        # Sets speed and angle
        rc.drive.set_speed_angle(rc_utils.clamp(speed, -1, 1),
                                 rc_utils.clamp(self.angle, -1, 1))
Ejemplo n.º 4
0
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)
Ejemplo n.º 5
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

    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)
Ejemplo n.º 6
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

    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)
Ejemplo n.º 7
0
    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
Ejemplo n.º 8
0
def update_ar(info, info_id, ar_image):
    global FIRST_PRI1
    global SECOND_PRI1
    global red_dir
    global blue_dir
    global green_dir
    red_dir = 0
    blue_dir = 0
    green_dir = 0

    #ar_image= rc_utils.crop(image, (0, rc.camera.get_width()//2), (rc.camera.get_height(), rc.camera.get_width()))

    if info is not None:  #checks if there are AR markers, then gets the contour of the outlining color
        rc_utils.draw_ar_markers(ar_image, info, info_id, (0, 255, 0))
        contours_ar_red = rc_utils.find_contours(ar_image, RED[0], RED[1])
        contours_ar_blue = rc_utils.find_contours(ar_image, BLUE[0], BLUE[1])
        contours_ar_green = rc_utils.find_contours(ar_image, GREEN[0],
                                                   GREEN[1])

        red_largest = rc_utils.get_largest_contour(contours_ar_red, 2000)
        blue_largest = rc_utils.get_largest_contour(contours_ar_blue, 2000)
        green_largest = rc_utils.get_largest_contour(contours_ar_green, 2000)

        if red_largest is not None:
            redc = rc_utils.get_contour_center(red_largest)
            red_dir = color_dir(redc, info, red_dir)
            rc_utils.draw_contour(ar_image, red_largest)
        if blue_largest is not None:
            bluec = rc_utils.get_contour_center(blue_largest)
            blue_dir = color_dir(bluec, info, blue_dir)
            rc_utils.draw_contour(ar_image, blue_largest)
        if green_largest is not None:
            greenc = rc_utils.get_contour_center(green_largest)
            green_dir = color_dir(greenc, info, green_dir)
            rc_utils.draw_contour(ar_image, green_largest)

    # if red_dir != 0 or green_dir != 0 or blue_dir != 0:
        if red_dir == 0:
            THIRD_PRI = red_dir
        elif blue_dir == 0:
            THIRD_PRI = blue_dir
        else:
            THIRD_PRI = green_dir
Ejemplo n.º 9
0
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])
Ejemplo n.º 10
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)
Ejemplo n.º 11
0
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)
Ejemplo n.º 12
0
def flane(color_lane):
    global contour_center
    global contour_area
    global speed
    global angle
    global lane
    global arcount
    global a
    global time
    time += rc.get_delta_time()

    cimage = rc.camera.get_color_image()
    if cimage is None:
        contour_center = None
        contour_area = 0
    else:
        #splits image in two so that two separate contours followed
        left = rc_utils.crop(
            cimage, (360, 0),
            (rc.camera.get_height(), rc.camera.get_width() // 2))
        right = rc_utils.crop(cimage, (360, rc.camera.get_width() // 2),
                              (rc.camera.get_height(), rc.camera.get_width()))
        both = [left, right]
        contour_centers = []
        contour_areas = []

        #find largest contours of left
        contours_pur = rc_utils.find_contours(left, color_lane[0],
                                              color_lane[1])
        print(contours_pur)
        rc.display.show_color_image(left)
        print(color_lane)
        contour_left = rc_utils.get_largest_contour(contours_pur,
                                                    MIN_CONTOUR_AREA)
        #if there are contours, finds center and adds to contour_centers list
        if contour_left is not None:
            left_center = rc_utils.get_contour_center(contour_left)
            contour_centers.append(left_center)
            rc_utils.draw_contour(left, contour_left, (255, 0, 0))
            rc.display.show_color_image(left)
        else:
            contour_centers.append(None)

        #find largest contours of right
        contours = rc_utils.find_contours(right, color_lane[0], color_lane[1])
        contour_right = rc_utils.get_largest_contour(contours,
                                                     MIN_CONTOUR_AREA)
        #if there are contours, finds center and adds to contour_centers list
        if contour_right is not None:
            right_center = rc_utils.get_contour_center(contour_right)
            contour_centers.append(right_center)
            #rc_utils.draw_contour(right, contour_right)
            #rc.display.show_color_image(right)
        else:
            contour_centers.append(None)

        #adjusts car based on being in the center of two contours
        if None not in contour_centers:
            lane = True
            image = rc.camera.get_color_image()
            contour_distance = (
                contour_centers[1][1] +
                rc.camera.get_width() // 2) - contour_centers[0][1]
            contour_center = (contour_centers[0][0] + 10,
                              (contour_distance // 2) + contour_centers[0][1])
        else:
            contour_center = None
            speed = 1
            angle = 0
        if lane == True:  #catches if goes off lane, else, adjusts based off of center of contours
            if contour_centers[0] == None:
                print("turn left")
                angle = -1
            elif contour_centers[1] == None:
                print("turn right")
                angle = 1
            else:
                angle = rc_utils.remap_range(contour_center[1], 0,
                                             rc.camera.get_width(), -1, 1,
                                             True)
                speed = 1

            ##getting ar direction
            if arcount == 0 and time > 2:
                a = rc.camera.get_color_image()
                a = rc_utils.crop(
                    a, (0, rc.camera.get_width() // 4),
                    (rc.camera.get_height(), rc.camera.get_width() -
                     (rc.camera.get_width() // 4)))
                corners, ids = rc_utils.get_ar_markers(a)
                rc_utils.draw_ar_markers(a, corners, ids)
                if len(corners) > 0:
                    #print("getting dir")
                    if rc_utils.get_ar_direction(corners[0]) == Direction.LEFT:
                        print(rc_utils.get_ar_direction(corners[0]))
                        angle = -0.7
                    if rc_utils.get_ar_direction(corners[0]):
                        print(rc_utils.get_ar_direction(corners[0]))
                        angle = 0.7
        speed = 1
        rc.drive.set_speed_angle(0, 0)
Ejemplo n.º 13
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