新增特定颜色物体跟踪

src/rasp_camera/scripts/color_recognition.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+import rospy, cv2, cv_bridge, math, datetime
+import numpy as np
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float64
+
+ball_color = 'red'
+
+color_dist = {'red': {'Lower': np.array([0, 60, 60]), 'Upper': np.array([6, 255, 255])},
+              'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])},
+              'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])},
+              'black': {'Lower': np.array([0, 0, 0]), 'Upper': np.array([180, 255, 46])},
+              'yellow': {'Lower': np.array([21, 39, 64]), 'Upper': np.array([34, 255, 255])},
+              }
+class Color_Recognition:
+
+    def __init__(self):
+        self.bridge = cv_bridge.CvBridge()
+        # cv2.namedWindow('window', 1)
+
+        self.image_sub = rospy.Subscriber('/raspicam_node/image', Image, self.image_callback)
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+
+
+        self.twist = Twist()
+        self.err = 0
+        self.cnt = 0
+
+        # self.lift_height_pub.publish(20)
+
+
+    def image_callback(self, msg):
+        # To get the image from the camera and convert it to binary image by using opencv
+        img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+
+        cv2.imshow('window_img',img)
+        cv2.waitKey(3)
+        
+        gs_frame = cv2.GaussianBlur(img, (5, 5), 0)
+        hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)                 # 转化成HSV图像
+        erode_hsv = cv2.erode(hsv, None, iterations=2)                   # 腐蚀 粗的变细
+        inRange_hsv = cv2.inRange(erode_hsv, color_dist[ball_color]['Lower'], color_dist[ball_color]['Upper'])
+        cnts = cv2.findContours(inRange_hsv.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
+
+        if len(cnts) == 0:
+            return
+        c = max(cnts, key=cv2.contourArea)
+        print("area:",cv2.contourArea(c))
+        rect = cv2.minAreaRect(c)
+        box = cv2.boxPoints(rect)
+        cv2.drawContours(img, [np.int0(box)], -1, (0, 255, 255), 2)
+
+        cv2.imshow('window_contours', img)
+        cv2.waitKey(3)
+        x_center = 160
+        current_center = 0.5*(box[2,0]-box[0,0])+(box[0,0])
+        err_center = current_center - x_center
+
+        area = 3000
+        current_area = cv2.contourArea(c)
+        err_area = current_area - area
+        
+        self.twist.linear.x = 0
+        self.twist.angular.z = 0
+
+        if abs(err_area)>500:
+            self.twist.linear.x = 0.0074 * -err_area * 0.006
+            print 'linear.x', self.twist.linear.x
+        if abs(err_center)>5:
+            self.twist.angular.z = 0.74 * -err_center * 0.006
+            print 'angular.z', self.twist.angular.z
+        self.cmd_vel_pub.publish(self.twist)
+
+
+if __name__ == '__main__':
+    rospy.init_node('color_recognition')
+    color_recognition = Color_Recognition()
+    rospy.spin()