find_cube.py

import cv2
import numpy as np
import time


def filter_image(img, hsv_lower, hsv_upper):
    img_filt = cv2.medianBlur(img, 11)
    hsv = cv2.cvtColor(img_filt, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv, hsv_lower, hsv_upper)
    return mask


    ###############################################################################
    ### You might need to change the parameter values to get better results
    ###############################################################################
def detect_blob(mask):
    '''img = cv2.medianBlur(mask, 9)
   # Set up the SimpleBlobdetector with default parameters.
    params = cv2.SimpleBlobDetector_Params()
    # Change thresholds
    params.minThreshold = 0;
    params.maxThreshold = 256;
    #filter by color (on binary)
    params.filterByColor = True
    params.blobColor = 255  # this looks at binary image 0 for looking for dark areas
    # Filter by Area.
    params.filterByArea = True
    params.minArea = 200
    params.maxArea = 20000
    # Filter by Circularity
    params.filterByCircularity = False
    # Filter by Convexity
    params.filterByConvexity = False
    # Filter by Inertia
    params.filterByInertia = False
    detector = cv2.SimpleBlobDetector_create(params)
    # Detect blobs.
    keypoints = detector.detect(img)
    return keypoints'''

    img = mask

    # Set up the SimpleBlobdetector with default parameters with specific values.
    params = cv2.SimpleBlobDetector_Params()

    params.filterByArea = True
    params.minArea = 300
    params.maxArea = 100000

    params.filterByColor = True
    params.blobColor = 255

    params.filterByInertia = False
    params.filterByConvexity = False
    params.filterByCircularity = False

    #ADD CODE HERE

    # builds a blob detector with the given parameters
    detector = cv2.SimpleBlobDetector_create(params)

    # use the detector to detect blobs.
    keypoints = detector.detect(img)
    keypointsImage = cv2.drawKeypoints(
        img, keypoints,
        np.array([]), (0, 0, 255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
    cv2.imshow("Blobs Detected", keypointsImage)
    return keypoints


def find_cube(img, hsv_lower, hsv_upper):
    """Find the cube in an image.
        Arguments:
        img -- the image
        hsv_lower -- the h, s, and v lower bounds
        hsv_upper -- the h, s, and v upper bounds
        Returns [x, y, radius] of the target blob, and [0,0,0] or None if no blob is found.
    """
    mask = filter_image(img, hsv_lower, hsv_upper)
    keypoints = detect_blob(mask)

    if keypoints == []:
        return None

    ###############################################################################
    # Todo: Sort the keypoints in a certain way if multiple key points get returned
    ###############################################################################

    #keypoints = sorted(keypoints, key=lambda keypoint: keypoint.size, reverse=True)
    return [keypoints[0].pt[0], keypoints[0].pt[1], keypoints[0].size]

go_to_cube.py

#!/usr/bin/env python3
#!c:/Python35/python3.exe -u
import asyncio
import sys
import cv2
import numpy as np
import cozmo
import time
import os
from glob import glob

from find_cube import *

try:
    from PIL import ImageDraw, ImageFont
except ImportError:
    sys.exit('run `pip3 install --user Pillow numpy` to run this example')


def nothing(x):
    pass


PINK_LOWER = np.array(np.array([168, 150, 141]).round(), np.uint8)
PINK_UPPER = np.array(np.array([180, 224, 255]).round(), np.uint8)

IMAGE_WIDTH = 320


# Define a decorator as a subclass of Annotator; displays the keypoint
class BoxAnnotator(cozmo.annotate.Annotator):

    cube = None

    def apply(self, image, scale):
        d = ImageDraw.Draw(image)
        bounds = (0, 0, image.width, image.height)

        if BoxAnnotator.cube is not None:

            #double size of bounding box to match size of rendered image
            BoxAnnotator.cube = np.multiply(BoxAnnotator.cube, 2)

            #define and display bounding box with params:
            #msg.img_topLeft_x, msg.img_topLeft_y, msg.img_width, msg.img_height
            box = cozmo.util.ImageBox(
                BoxAnnotator.cube[0] - BoxAnnotator.cube[2] / 2,
                BoxAnnotator.cube[1] - BoxAnnotator.cube[2] / 2,
                BoxAnnotator.cube[2], BoxAnnotator.cube[2])
            cozmo.annotate.add_img_box_to_image(image, box, "green", text=None)

            BoxAnnotator.cube = None


async def run(robot: cozmo.robot.Robot):

    robot.world.image_annotator.annotation_enabled = True
    robot.world.image_annotator.add_annotator('box', BoxAnnotator)

    robot.camera.image_stream_enabled = True
    robot.camera.color_image_enabled = True
    robot.camera.enable_auto_exposure = True

    await robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()

    gain, exposure, mode = 390, 3, 1

    lowerThreshold = PINK_LOWER
    upperThreshold = PINK_UPPER

    def callback(*args):
        return
        #lowerThreshold = np.array([cv2.getTrackbarPos("Hue Lower", windowName), cv2.getTrackbarPos("Sat Lower", windowName), cv2.getTrackbarPos("Val Lower", windowName)])
        #upperThreshold = np.array([cv2.getTrackbarPos("Hue Upper", windowName), cv2.getTrackbarPos("Sat Upper", windowName), cv2.getTrackbarPos("Val Upper", windowName)])

    windowName = "Threshold Adjuster"
    cv2.namedWindow(windowName)
    cv2.createTrackbar("Hue Lower", windowName, lowerThreshold[0], 180,
                       callback)
    cv2.createTrackbar("Hue Upper", windowName, upperThreshold[0], 180,
                       callback)
    cv2.createTrackbar("Sat Lower", windowName, lowerThreshold[1], 255,
                       callback)
    cv2.createTrackbar("Sat Upper", windowName, upperThreshold[1], 255,
                       callback)
    cv2.createTrackbar("Val Lower", windowName, lowerThreshold[2], 255,
                       callback)
    cv2.createTrackbar("Val Upper", windowName, upperThreshold[2], 255,
                       callback)

    last_turn = 0
    oscillations = 0
    near_mode = False
    last_diameters = []

    try:

        while True:
            cv2.waitKey(1)

            lowerThreshold = np.array([
                cv2.getTrackbarPos("Hue Lower", windowName),
                cv2.getTrackbarPos("Sat Lower", windowName),
                cv2.getTrackbarPos("Val Lower", windowName)
            ])
            upperThreshold = np.array([
                cv2.getTrackbarPos("Hue Upper", windowName),
                cv2.getTrackbarPos("Sat Upper", windowName),
                cv2.getTrackbarPos("Val Upper", windowName)
            ])

            event = await robot.world.wait_for(
                cozmo.camera.EvtNewRawCameraImage,
                timeout=30)  #get camera image
            if event.image is not None:
                image = cv2.cvtColor(
                    np.asarray(event.image), cv2.COLOR_BGR2RGB)
                if mode == 1:
                    robot.camera.enable_auto_exposure = True
                else:
                    robot.camera.set_manual_exposure(exposure, fixed_gain)

                #find the cube
                cube = find_cube(image, lowerThreshold, upperThreshold)
                print(cube)
                BoxAnnotator.cube = cube

                if (near_mode):
                    # we're near the box, blob detection gets glitchy
                    # keep track of last 5 diameters, if the average falls below drop out of this mode
                    print(last_diameters)
                    if cube == None:
                        last_diameters.append(0)
                    else:
                        last_diameters.append(cube[2])
                    if len(last_diameters) >= 41:
                        last_diameters = last_diameters[1:41]
                    if np.average(np.array(last_diameters)) > 100:
                        continue
                    else:
                        print("dropping out of near mode")
                        last_diameters = []
                        near_mode = False

                # if no keypoint, start turning til there is one
                if (cube is None):
                    await robot.turn_in_place(
                        cozmo.util.degrees(37)).wait_for_completed()
                else:  # turn until it is in the center
                    delta = (IMAGE_WIDTH / 2) - cube[0]
                    oscillations += (last_turn == np.sign(delta) * -1)
                    if abs(delta) > 30:
                        last_turn = np.sign(delta)
                        # assume <15 oscillations
                        await robot.turn_in_place(
                            cozmo.util.degrees(
                                np.sign(delta) * np.max([
                                    delta / 7 - oscillations, 5
                                ]))).wait_for_completed()
                    else:
                        if cube[2] < 180:
                            await robot.drive_straight(
                                cozmo.util.distance_inches(3),
                                cozmo.util.speed_mmps(800)
                            ).wait_for_completed()
                        else:
                            near_mode = True

    except KeyboardInterrupt:
        print("")
        print("Exit requested by user")
    except cozmo.RobotBusy as e:
        print(e)
    #cv2.destroyAllWindows()


if __name__ == '__main__':
    cozmo.run_program(run, use_viewer=True, force_viewer_on_top=True)