基于ros框架和opencv颜色捕捉功能，实现机器人充当守门员

首先展示实现效果，AI守门员请求出战：https://v.douyin.com/erE3dJo/

实现条件：

1.有一个ROS框架的麦克纳姆轮智能机器人（可左右横向移动的机器人）；

2.能够发布摄像头信息；

3.对opencv的基本API有一定的了解。

算法思路：

1.订阅摄像头信息，用cv_bridge转换为可处理的opencv图；

2.通过opencv的颜色过滤功能，实时捕捉“足球”的位置；

3..根据足球位置和PID算法，移动机器人，使机器人始终保持在“足球”的正前方。

可结合注释阅读核心源代码（摄像头发布代码和机器人移动代码，因机器人种类的不同，不能一概而论，因此这里就谈了）：

#!/usr/bin/env python
#-*- coding: utf-8 -*-
from __future__ import print_function


import sys
import rospy
import time
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import Twist, Vector3

class image_converter:

  def __init__(self):
    # 定义处理前图像订阅器
    self.image_sub = rospy.Subscriber("/usb_cam/image_raw",Image,self.callback)
    # 定义处理后图像发布器
    self.image_pub = rospy.Publisher("/image_topic_2",Image, queue_size =3)
    # 定义原图和opencv图转换器
    self.bridge = CvBridge()
    # 定义机器人速度控制器
    self.cmdVelPublisher = rospy.Publisher('/cmd_vel', Twist, queue_size =3)
    # 定义PID，其中320为视频X方向中心点。P=0.005
    self.PID_controller_distance = simplePID(320, 0.005,0, 0)

  def callback(self,data):
    # 将原图转为opencv图
    try:
      frame = self.bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
      print(e)
    # 选取“足球”红色的取值范围
    color_lower=np.array([0,43,46])
    color_upper = np.array([10, 255, 255])
    # 捕捉“足球”
    frame_=cv2.GaussianBlur(frame,(5,5),0)                    
    hsv=cv2.cvtColor(frame_,cv2.COLOR_BGR2HSV)
    mask=cv2.inRange(hsv,color_lower,color_upper)  
    mask=cv2.erode(mask,None,iterations=2)
    mask=cv2.dilate(mask,None,iterations=2)
    mask=cv2.GaussianBlur(mask,(3,3),0)     
    cnts=cv2.findContours(mask.copy(),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2] 
    if len(cnts)>0:
      cnt = max (cnts,key=cv2.contourArea)
      (color_x,color_y),color_radius=cv2.minEnclosingCircle(cnt)
      if color_radius > 20:
        cv2.circle(frame,(int(color_x),int(color_y)),int(color_radius),(255,0,255),2) 
        print(color_x,color_y,color_radius)
        # 将opencv图转为原图并发布
        try:
          self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))
        except CvBridgeError as e:
          print(e)
      # PID控制，让小车始终保持在球的正前方
        go_distance = self.PID_controller_distance.update(color_x)
        # rospy.loginfo("pid_distance:{}".format(go_distance))
        velocity = Twist()
        velocity.linear = Vector3(0,go_distance,0)
        velocity.angular= Vector3(0., 0.,0.)
        self.cmdVelPublisher.publish(velocity)
    # (rows,cols,channels) = frame.shape
    # print(rows,cols,channels)

class simplePID:
	'''very simple discrete PID controller'''
	def __init__(self, target, P, I, D):
        # rospy.loginfo('PID initialised with P:{}, I:{}, D:{}'.format(P,I,D))
		self.Kp		=np.array(P)
		self.Ki		=np.array(I)
		self.Kd		=np.array(D)
		self.setPoint   =np.array(target)
		
		self.last_error=0
		self.integrator = 0
		self.integrator_max = float('inf')
		self.timeOfLastCall = None 
		
		
	def update(self, current_value):
		current_value=np.array(current_value)
		if(self.timeOfLastCall is None):
			# the PID was called for the first time. we don't know the deltaT yet
			# no controll signal is applied
			self.timeOfLastCall = time.clock()
			return np.zeros(np.size(current_value))

		
		error = self.setPoint - current_value
		P =  error
		# rospy.loginfo(P)
		currentTime = time.clock()
		deltaT      = (currentTime-self.timeOfLastCall)

		# integral of the error is current error * time since last update
		self.integrator = self.integrator + (error*deltaT)
		I = self.integrator
		
		# derivative is difference in error / time since last update
		D = (error-self.last_error)/deltaT
		
		self.last_error = error
		self.timeOfLastCall = currentTime
		
		# return controll signal
		return self.Kp*P + self.Ki*I + self.Kd*D


if __name__ == '__main__':
  rospy.init_node('image_converter', anonymous=True)
  ic = image_converter()
  print('start')
  try:
    rospy.spin()
  except rospy.ROSInterruptException:
    print('exception')