pub middle

image_track.py

@@ -9,7 +9,7 @@ import math
 from skimage import measure
 from cv_bridge import CvBridge, CvBridgeError
 from sensor_msgs.msg import Image
-from std_msgs.msg import String
+from std_msgs.msg import Float64
 
 
 import datetime
@@ -22,14 +22,9 @@ class Follower:
         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.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+        self.line_pub = rospy.Publisher('/man3/middle', Float64, 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
@@ -62,53 +57,32 @@ class Follower:
 
         # 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
+            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)         
 
         cv2.imshow('window1', image)
         cv2.waitKey(1)