SHARE
TWEET

Untitled

a guest Feb 22nd, 2019 81 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2.  
  3. import numpy
  4.  
  5. import cv2
  6. import cv_bridge
  7. import rospy
  8.  
  9. from sensor_msgs.msg import Image
  10. from geometry_msgs.msg import Twist
  11.  
  12.  
  13. class Follower:
  14.     def __init__(self):
  15.         self.bridge = cv_bridge.CvBridge()
  16.         self.image_sub = rospy.Subscriber('/camera/rgb/image_raw', Image,
  17.                                           self.image_callback)
  18.         self.cmd_vel_pub = rospy.Publisher('/mobile_base/commands/velocity', Twist,
  19.                                            queue_size=1)
  20.         self.twist = Twist()
  21.  
  22.     def image_callback(self, msg):
  23.         cv2.namedWindow("window", 1)
  24.         image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
  25.         hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
  26.         lower_yellow = numpy.array([10, 10, 10])
  27.         upper_yellow = numpy.array([255, 255, 255])
  28.         mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
  29.         h, w, d = image.shape
  30.         search_top = 3*h/4
  31.         search_bot = 3*h/4 + 20
  32.         mask[0:search_top, 0:w] = 0
  33.         mask[search_bot:h, 0:w] = 0
  34.         M = cv2.moments(mask)
  35.         if M['m00'] > 0:
  36.             cx = int(M['m10']/M['m00'])
  37.             cy = int(M['m01']/M['m00'])
  38.             cv2.circle(image, (cx, cy), 20, (0, 0, 255), -1)
  39.             err = cx - w/2
  40.             self.twist.linear.x = 0.2
  41.         #if [err != 0.0]:
  42.         #self.twist.angular.z = 0.5
  43.             self.twist.angular.z = -float(err) / 100
  44.             print self.twist.angular.z
  45.  
  46.             self.cmd_vel_pub.publish(self.twist)
  47.         cv2.imshow("window", image)
  48.         cv2.waitKey(3)
  49.  
  50.  
  51. cv2.startWindowThread()
  52. rospy.init_node('follower')
  53. follower = Follower()
  54. rospy.spin()
  55.  
  56. cv2.destroyAllWindows()
  57.  
  58. #this is the follow.py file
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top