树莓派 智能小车 自动抓取乒乓球

（接上篇博客[Arduino 智能机器人 按指令行走]）(http://blog.youkuaiyun.com/qazwyc/article/details/56969383)

所需硬件

• 智能小车
• 树莓派3B
• Pi Camera

环境配置

• 安装opencv

sudo apt-get update
sudo apt-get upgrade
sudo apt-get install python-opencv

• 安装PiCamera来控制摄像头

sudo apt-get install python-pip 
sudo apt-get install python-dev 
sudo pip install picamera

代码

import cv2
import cv2.cv as cv
import cv
import picamera
import picamera.array
import math
import numpy as np
import zmq
import time
import serial, time, sys
from PIL import Image


# For OpenCV2 image display
IMAGE_WINDOW_NAME = 'YelloBarTracker'
CONTROL_WINDOW_NAME = 'Control'
MASK_WINDOW_NAME = 'Mask'

# For socket communication
port = '5556'
context = zmq.Context()
socket = context.socket(zmq.PUB)

# Setting the initial mask threshold 
# 根据环境调试数据以便准确的捕捉小球
iLowH = 5
iHighH = 22

iLowS = 219
iHighS = 255

iLowV = 149
iHighV = 255

# 是否抓到球
getball = 0

# connect arduino
# 将arduino接到树莓派上,输入ls /dev查看,我的是ttyUSB0
def connect_arduino():
    arduino = serial.Serial('/dev/ttyUSB0',9600,timeout=1)
    arduino.close()
    arduino.open()
    return arduino

# Require by cv2.createTrackbar. we have nothing to do with nothing method
def nothing(var):
    pass

def connect():
    print('Getting data from camera...')
    socket.bind('tcp://*:%s' % port)

# Create trackbars for easier adjustment of the HSV threshold  方便手动调节区间
def make_hsv_adjustment():
    cv2.namedWindow(CONTROL_WINDOW_NAME)
    cv2.createTrackbar('LowH', CONTROL_WINDOW_NAME, iLowH, 255, nothing); #Hue (0 - 179)
    cv2.createTrackbar('HighH', CONTROL_WINDOW_NAME, iHighH, 255, nothing);

    cv2.createTrackbar('LowS', CONTROL_WINDOW_NAME, iLowS, 255, nothing); #Saturation (0 - 255)
    cv2.createTrackbar('HighS', CONTROL_WINDOW_NAME, iHighS, 255, nothing);

    cv2.createTrackbar('LowV', CONTROL_WINDOW_NAME, iLowV, 255, nothing); #Value (0 - 255)
    cv2.createTrackbar('HighV', CONTROL_WINDOW_NAME, iHighV, 255, nothing);

def track(image):

    '''Accepts BGR image as Numpy array
       Returns: (x,y) coordinates of centroid if found
                (-1,-1) if no centroid was found
                None if user hit ESC
    '''

    # Blur the image to reduce noise
    blur = cv2.GaussianBlur(image, (5,5),0)

    # Convert BGR to HSV
    hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)

    # Get the treshold from the trackbars
    iLowH = cv2.getTrackbarPos('LowH', CONTROL_WINDOW_NAME)
    iHighH = cv2.getTrackbarPos('HighH', CONTROL_WINDOW_NAME)
    iLowS = cv2.getTrackbarPos('LowS', CONTROL_WINDOW_NAME)
    iHighS = cv2.getTrackbarPos('HighS', CONTROL_WINDOW_NAME)
    iLowV = cv2.getTrackbarPos('LowV', CONTROL_WINDOW_NAME)
    iHighV = cv2.getTrackbarPos('HighV', CONTROL_WINDOW_NAME)

    # Threshold the HSV image for only green colors
    lower_yellow = np.array([iLowH,iLowS,iLowV])
    upper_yellow = np.array([iHighH,iHighS,iHighV])

    # Threshold the HSV image to get only yellow colors
    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
    cv2.imshow(MASK_WINDOW_NAME, mask)

    # Blur the mask
    bmask = cv2.GaussianBlur(mask, (5,5),0)

    # Take the moments to get the centroid
    moments = cv2.moments(bmask)
    m00 = moments['m00']
    centroid_x, centroid_y, radius = None, None, None
    if m00 != 0:
        centroid_x = int(moments['m10']/m00)
        centroid_y = int(moments['m01']/m00)
        radius = int(math.sqrt(m00 / 255 / 3.14159265358979323846))

    # Assume no centroid
    ball = (-1,-1, 0)

    # Use centroid if it exists
    if centroid_x != None and centroid_y != None and radius != None:

        ball = (centroid_x, centroid_y, radius) # 中心坐标及半径

        # Put red circle in at centroid in image
        cv2.circle(image, (centroid_x, centroid_y), 4, (255,0,0)) # center
        cv2.circle(image, (centroid_x, centroid_y), radius, (0,255,0))

    # Display full-color image
    cv2.imshow(IMAGE_WINDOW_NAME, image)

    # Force image display, setting centroid to None on ESC key input
    if cv2.waitKey(1) & 0xFF == 27:
        ball = None

    # Return coordinates of ball
    return ball

if __name__ == '__main__':
    connect()
    make_hsv_adjustment();
    arduino = connect_arduino();
    with picamera.PiCamera() as camera:
        with picamera.array.PiRGBArray(camera) as stream:
            camera.resolution = (320, 240)

            while True:
                camera.capture(stream, 'bgr', use_video_port=True)
                # stream.array now contains the image data in BGR order
                image = stream.array
                ball = track(image)
                if not ball:
                    break
                if cv2.waitKey(1) & 0xFF == 27:
                    break

                msg = '%d %d %d' % ball
                print(msg)
                socket.send(msg)

                # try
                # 未抓到球时发现距离较近,尝试抓取 
                if ball[2] > 55 and getball == 0:
                  # 前进一步
                  arduino.write('1')
                  # 夹取
                  arduino.write('s')
                  # 假设夹取到
                  getball = 1
                # 已成功夹取到
                elif ball[2] > 55 and getball == 1:
                    # 小车停止
                    arduino.write('9')
                    break;
                # 未成功抓取 释放夹手
                else:
                  arduino.write('r')
                  getball = 0

                if getball == 0:
                    # 前方未发现有小球
                    if ball[2] <= 2:
                        # 原地右转
                        arduino.write('8')
                        arduino.write('9')
                    else:
                        # 小球在前方偏右 (摄像头安装时是倒的)
                        if ball[0] < 130 and ball[2] <= 20:
                            # 原地右转
                            arduino.write('8')
                            arduino.write('9')
                        # 小球在前方偏左
                        elif ball[0] > 190 and ball[2] <= 20:
                            # 原地左转
                            arduino.write('7')
                            arduino.write('9')
                        else:
                            arduino.write('1')

                # reset the stream before the next capture
                stream.seek(0)
                stream.truncate()

                time.sleep(1);

            # 完成夹取小球的任务后可以通过在控制台敲入指令控制小车
            while True:
                cmd = sys.stdin.readline()
                arduino.write(cmd)

            cv2.destroyAllWindows()

（实训时做的,水平有限，易受环境光照影响，小车找寻与抓取速度也较慢，仅供参考）