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

我不是女神ヾ 2022-09-17 04:20 95阅读 0赞

转载自：[基于ros框架和opencv颜色捕捉功能，实现机器人充当守门员\_彭艺的博客-CSDN博客][ros_opencv_-CSDN]

实现条件：

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')

[ros_opencv_-CSDN]: https://blog.csdn.net/weixin_44122258/article/details/115904929?spm=1001.2101.3001.6650.6&utm_medium=distribute.pc_relevant.none-task-blog-2~default~BlogCommendFromBaidu~default-6.no_search_link&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2~default~BlogCommendFromBaidu~default-6.no_search_link