运动控制¶

1.视觉处理¶

本章我们来学习如何控制小车运动,我们首先看下下面的这个需求.在小车的前面摆放着一个黄色的雪糕筒,我们希望当我移动雪糕筒的时候,小车也能够跟着移动,这个该如何来做呢!

我们可以用所学过的知识来做, 首先我们要通过摄像来识别出黄色的雪糕筒,这个可能得用到我们前面所学的HSV颜色空间.

根据雪糕筒,小车可做的运动有如下3种情况:

若雪糕筒,出现在画面的左边,则小车左转.
若雪糕筒出现在画面的中间区域,则小车直行
若雪糕筒出现在画面的右边,则小车右转

依据上面的分析,我们其实可以先来编写机器视觉部分的代码,示例参考如下:

import cv2 as cv

capture = cv.VideoCapture(0)

ok,frame = capture.read()

lowerb = (23,43,46)
upperb = (34,255,255)

while ok:

    hsv_frame =cv.cvtColor(frame,cv.COLOR_BGR2HSV)

    mask = cv.inRange(hsv_frame,lowerb,upperb)

    cv.imshow("mask",mask)
    cv.imshow("frame",frame)
    cv.waitKey(10)
    ok, frame = capture.read()

2. 小车跟随颜色块¶

这个案例,我们让小车跟随黄色块进行运动,当然如果大家在地面上贴一根黄色的线,小车也可以跟随黄色线进行运动.

当前这个案例相对来说,还是比较容易,后面我们会在这个案例的基础上进一步学习.

#! /usr/bin/env python
# encoding: utf-8
import rospy
from geometry_msgs.msg import Twist
import cv2 as cv

def shutdown():
    twist = Twist()
    twist.linear.x = 0
    twist.angular.z = 0
    cmd_vel_Publisher.publish(twist)
    print "stop car..."

if __name__ == '__main__':
    rospy.init_node("yellow_follow")

    # 当程序退出
    rospy.on_shutdown(shutdown);

    # ros控制的频率
    rate = rospy.Rate(100)

    # 定义publisher : cmd_vel
    cmd_vel_Publisher = rospy.Publisher("/cmd_vel",Twist,queue_size=1)



    capture = cv.VideoCapture(0)
    print capture.isOpened()

    ok,frame = capture.read()

    lowerb = (23,43,46)
    upperb = (34,255,255)

    height,width = frame.shape[0:2]
    screen_center = width / 2
    offset = 50
    while not rospy.is_shutdown():

        # 将图像转成HSV颜色空间
        hsv_frame = cv.cvtColor(frame, cv.COLOR_BGR2HSV)
        # 基于颜色的物品提取
        mask = cv.inRange(hsv_frame,lowerb,upperb)
        # 找出面积最大的区域
        _,contours,_ = cv.findContours(mask,cv.RETR_EXTERNAL,cv.CHAIN_APPROX_SIMPLE)

        maxArea = 0
        maxIndex = 0
        for i,c in enumerate(contours):
            area = cv.contourArea(c)
            if area > maxArea:
                maxArea = area
                maxIndex = i
        # 绘制
        cv.drawContours(frame,contours,maxIndex,(0,0,255),2)
        # 获取外切矩形
        x,y,w,h = cv.boundingRect(contours[maxIndex])
        cv.rectangle(frame,(x,y),(x+w,y+h),(255,255,0),2)

        # 获取外切矩形的中心像素点
        center_x = int(x + w/2)
        center_y = int(y + h/2)
        cv.circle(frame,(center_x,center_y),5,(0,0,255),-1)

        # 判断当前小车应该是左转还是右转还是直行
        twist = Twist()
        if center_x < screen_center - offset:
            twist.linear.x = 0.05
            twist.angular.z = 0.2
            print "turn left"
        elif center_x >= screen_center - offset and center_x <= screen_center + offset:
            twist.linear.x = 0.1
            twist.angular.z = 0.0
            print "go"
        elif center_x >  screen_center + offset:
            twist.linear.x = 0.05
            twist.angular.z = -0.2
            print "turn right"
        else:
            twist.linear.x = 0
            twist.angular.z = 0
            print "stop"

        # 将速度信息发送出去
        cmd_vel_Publisher.publish(twist)

        cv.imshow("mask",mask)
        cv.imshow("frame",frame)
        cv.waitKey(1)
        rate.sleep()

        ok, frame = capture.read()