forked from salabson/ros_book_sample_code
-
Notifications
You must be signed in to change notification settings - Fork 2
/
follower_p.py
executable file
·55 lines (44 loc) · 1.6 KB
/
follower_p.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
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
#!/usr/bin/env python
import numpy
import cv2
import cv_bridge
import rospy
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
class Follower:
def __init__(self):
self.bridge = cv_bridge.CvBridge()
self.image_sub = rospy.Subscriber('/camera/rgb/image_raw', Image,
self.image_callback)
self.cmd_vel_pub = rospy.Publisher('/mobile_base/commands/velocity', Twist,
queue_size=1)
self.twist = Twist()
def image_callback(self, msg):
cv2.namedWindow("window", 1)
image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_yellow = numpy.array([10, 10, 10])
upper_yellow = numpy.array([255, 255, 250])
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
h, w, d = image.shape
search_top = 3*h/4
search_bot = 3*h/4 + 20
mask[0:search_top, 0:w] = 0
mask[search_bot:h, 0:w] = 0
M = cv2.moments(mask)
if M['m00'] > 0:
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
cv2.circle(image, (cx, cy), 20, (0, 0, 255), -1)
err = cx - w/2
self.twist.linear.x = 0.2
self.twist.angular.z = -float(err) / 100
print self.twist.angular.z
self.cmd_vel_pub.publish(self.twist)
cv2.imshow("window", image)
cv2.waitKey(3)
cv2.startWindowThread()
rospy.init_node('follower')
follower = Follower()
rospy.spin()
cv2.destroyAllWindows()