Skip to content

GitLab

Switch branch/tag
Find file Normal viewHistoryPermalink
image_track.py 4.08 KB
Newer Older
he's avatar
From Kuang  
he committed
1
2
3
4
5
6
7
8
9
10
11 

# 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
he's avatar
pub middle    
he committed
12 
from std_msgs.msg import Float64
he's avatar
From Kuang  
he committed
13
14
15
16
17
18
19
20
21
22
23
24 


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)
he's avatar
pub middle    
he committed
25
26 
        #self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.line_pub = rospy.Publisher('/man3/middle', Float64, queue_size=1)
he's avatar
From Kuang  
he committed
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59 


    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:
he's avatar
pub middle    
he committed
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85 
            msg = Float64()
            msg.data = -1
            self.line_pub.publish(msg)    
        else:

            sort = np.argsort(areas)
            index = sort[-1]

            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.drawContours(image, contours, index, (0, 0, 255), 3)
                cv2.circle(image, (cx, cy), 20, (255, 0, 0), -1)

                # P-controller
                msg = Float64()
                msg.data = cx
                self.line_pub.publish(msg)
            else:
                msg = Float64()
                msg.data = -1
                self.line_pub.publish(msg)
he's avatar
From Kuang  
he committed
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146 

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