From Kuang

image_track.py

+
+# coding=utf-8
+import rospy
+import cv2
+import numpy as np
+import matplotlib.pyplot as plt
+import random
+import math
+from skimage import measure
+from cv_bridge import CvBridge, CvBridgeError
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+
+
+import datetime
+from geometry_msgs.msg import Twist
+
+
+class Follower:
+
+    def __init__(self):
+        self.bridge = CvBridge()
+
+        self.image_sub = rospy.Subscriber('/d415/color/image_raw', Image, self.image_callback)
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+
+        self.twist = Twist()
+        self.err = 0
+        self.turn_cmd = Twist()
+
+        self.signal = False
+        self.begin = 0
+
+    def image_callback(self, msg):
+        # To get the image from the camera and convert it to binary image by using opencv
+        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+
+        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
+
+        # Define the threshold of red and green
+        lower_red = np.array([170, 43, 40])
+        upper_red = np.array([180, 255, 255])
+
+
+        # Segment red line and green line from hsv image and convert it to  binary image
+        mask_red = cv2.inRange(hsv, lower_red, upper_red)
+
+        mask = mask_red
+        mask = cv2.erode(mask, (3,3))
+        h, w, d = image.shape
+        search_top = int(h / 2)
+        search_bot = search_top + 100
+        mask[0:search_top, :] = 0
+        #mask[search_bot:h, :] = 0
+        mask, contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+        areas = []
+
+
+        for cont in contours:
+            area = cv2.contourArea(cont)
+            areas.append(area)
+
+        # To find the optimal index of contour
+        if len(areas) == 0:
+            return False
+
+        sort = np.argsort(areas)
+        index = sort[-1]
+        print len(areas)
+        cv2.drawContours(image, contours, index, (0, 0, 255), 3)
+
+
+        if cv2.contourArea(contours[index]) >= 100:
+            # Computing the centroid of the contours of binary image
+            M = cv2.moments(contours[index])
+            self.signal = True
+            cx = int(M['m10'] / M['m00'])
+            cy = int(M['m01'] / M['m00'])
+            cv2.circle(image, (cx, cy), 20, (255, 0, 0), -1)
+
+            # P-controller
+            self.err = cx - w/2
+            self.twist.linear.x = 0.4
+            self.twist.angular.z = -float(self.err) / 45
+            self.cmd_vel_pub.publish(self.twist)
+
+            self.begin = datetime.datetime.now().second
+        # else:
+        #     # Define actions when the point is not be found
+        #     # 5 HZ
+        #     r = rospy.Rate(5)
+        #     # let's go forward at 0.1 m/s
+        #     # by default angular.z is 0 so setting this isn't required
+        #     move_cmd = Twist()
+        #     move_cmd.linear.x = 0.1
+
+        #     if self.signal is True:
+        #         if self.err < 0:
+        #             self.turn_cmd.angular.z = math.radians(30)
+        #         elif self.err > 0:
+        #             self.turn_cmd.angular.z = math.radians(-30)
+
+        #         self.cmd_vel_pub.publish(self.turn_cmd)
+        #         now = datetime.datetime.now().second
+
+        #         # Go forward about 0.2m, then stop
+        #         if math.fabs(now - self.begin) > 2:
+        #             for x in range(0, 10):
+        #                 self.cmd_vel_pub.publish(move_cmd)
+        #                 r.sleep()
+        #             self.signal = False
+
+        cv2.imshow('window1', image)
+        cv2.waitKey(1)
+        #cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    rospy.init_node('follow')
+    follow = Follower()
+    rospy.spin()
+
+
+
+
+
+
+
+    # lower_red = np.array([170, 43, 40])
+    # upper_red = np.array([180, 255, 255])
+
+    # for i in range(1,100):
+    #     dir = "./color/" + str(i) +".jpg"
+    #     image = cv2.imread(dir)
+    #     hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
+    #     mask = cv2.inRange(hsv, lower_red, upper_red)
+
+    #     mask = cv2.erode(mask, (3,3))
+    #     h, w, d = image.shape
+    #     search_top = 2 * h / 5
+    #     search_bot = search_top + 100
+    #     mask[0:search_top, :] = 0
+    #     #mask[search_bot:h, :] = 0
+    #     mask, contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+    #     areas = []
+
+
+    #     for cont in contours:
+    #         area = cv2.contourArea(cont)
+    #         areas.append(area)
+
+    #     # To find the optimal index of contour
+    #     if len(areas) == 0:
+    #         continue
+
+    #     sort = np.argsort(areas)
+    #     index = sort[-1]
+    #     print len(areas)
+    #     cv2.drawContours(image, contours, index, (0, 0, 255), 3)
+
+    #     if cv2.contourArea(contours[index]) <= 100:
+    #         continue
+
+    #     M = cv2.moments(contours[index])
+    #     cx = int(M['m10'] / M['m00'])
+    #     cy = int(M['m01'] / M['m00'])
+    #     cv2.circle(image, (cx, cy), 20, (255, 0, 0), -1)
+
+   
+    #     cv2.imshow('image', image)
+    #     cv2.waitKey(0)
+    #     cv2.destroyAllWindows()
\ No newline at end of file