ROSTHAI สร้างหุ่นยนต์ด้วย ROS: หุ่นยนต์ติดตามวัตถุ (ลูกปิงปอง) ใช้ ROS

....

1. สร้างไฟล์ x_sensor.py

เปิดโปรแกรม IDLE (using Python-2.7) คลิก New File เพื่อสร้างไฟล์ใหม่

เขียนโค้ดดังนี้


#!/usr/bin/env python
import cv2
import numpy as np
import rospy
from std_msgs.msg import Float32
from collections import deque
import argparse
import imutils
import sys


def talker():
 rospy.init_node('x_sensor', anonymous=True)
 pub=rospy.Publisher('x_coordinates',Float32,queue_size=1)
 rate = rospy.Rate(20) # 10hz
        ap = argparse.ArgumentParser()
 ap.add_argument("-v", "--video", dest="/home/robot/catkin_ws/src/robot_pkg/scripts/ball_tracking_example.mp4",help="path")
 ap.add_argument("-b", "--buffer", type=int, default=64,help="max buffer size")
 args = vars(ap.parse_args())

 greenLower = (20, 100, 100)
 greenUpper = (64, 255, 255)
 pts = deque(maxlen=64)
 if not args.get("video", False):
  camera = cv2.VideoCapture(0)
 else:
  camera = cv2.VideoCapture('args["video"]')

 while not rospy.is_shutdown():
  (grabbed, frame) = camera.read()
  if args.get("video") and not grabbed:
   break
  frame = imutils.resize(frame, width=533)
  frame = imutils.rotate(frame, angle=90)
  hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
  mask = cv2.inRange(hsv, greenLower, greenUpper)
  mask = cv2.erode(mask, None, iterations=2)
  mask = cv2.dilate(mask, None, iterations=2)
  cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2]
  center = None
  if len(cnts) > 0:
   c = max(cnts, key=cv2.contourArea)
   ((x, y), radius) = cv2.minEnclosingCircle(c)
   M = cv2.moments(c)
   center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
   #print(center)
   pub.publish(int(M["m01"] / M["m00"]))
   rospy.loginfo(int(M["m01"] / M["m00"]))
   rate.sleep()
   if radius > 10:
    cv2.circle(frame, (int(x), int(y)), int(radius),(0, 255, 255), 2)
    cv2.circle(frame, center, 5, (0, 0, 255), -1)
  pts.appendleft(center)
  for i in xrange(1, len(pts)):
   if pts[i - 1] is None or pts[i] is None:
    continue
   thickness = int(np.sqrt(64 / float(i + 1)) * 2.5)
   cv2.line(frame, pts[i - 1], pts[i], (0, 0, 255), thickness)

  cv2.imshow("Frame", frame)
  if cv2.waitKey(1) & 0xFF==ord('q'):
   break

 rospy.spin()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
 video_capture.release()
 cv2.destroyAllWindows()
 pass

หรือ ดาวน์โหลด ได้ที่

https://drive.google.com/open?id=1JdAgk75DMnbPNBrnN5OmOiXb3gvGbHaq

ไฟล์ : x_sensor.py

เขียนโค้ดเสร็จแล้ว Save As.. เก็บไฟล์ไว้ที่ โฟลเดอร์ robot_pkg/scripts

ต้องการให้เป็นไฟล์ที่ ROS เรียกใช้งานได้ โดยคำสั่ง:

cd catkin_ws/src/robot_pkg/scripts

chmod u+x x_sensor.py

2. สร้างไฟล์ dc_motor.py

เปิดโปรแกรม IDLE (using Python-2.7) คลิก New File เพื่อสร้างไฟล์ใหม่

เขียนโค้ดดังนี้


#!/usr/bin/env python
import cv2
import numpy as np
import rospy
from std_msgs.msg import Float32
from collections import deque
import argparse
import imutils
import sys


def talker():
 rospy.init_node('x_sensor', anonymous=True)
 pub=rospy.Publisher('x_coordinates',Float32,queue_size=1)
 rate = rospy.Rate(20) # 10hz
        ap = argparse.ArgumentParser()
 ap.add_argument("-v", "--video", dest="/home/robot/catkin_ws/src/robot_pkg/scripts/ball_tracking_example.mp4",help="path")
 ap.add_argument("-b", "--buffer", type=int, default=64,help="max buffer size")
 args = vars(ap.parse_args())

 greenLower = (20, 100, 100)
 greenUpper = (64, 255, 255)
 pts = deque(maxlen=64)
 if not args.get("video", False):
  camera = cv2.VideoCapture(0)
 else:
  camera = cv2.VideoCapture('args["video"]')

 while not rospy.is_shutdown():
  (grabbed, frame) = camera.read()
  if args.get("video") and not grabbed:
   break
  frame = imutils.resize(frame, width=800)
  frame = imutils.rotate(frame, angle=90)
  hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
  mask = cv2.inRange(hsv, greenLower, greenUpper)
  mask = cv2.erode(mask, None, iterations=2)
  mask = cv2.dilate(mask, None, iterations=2)
  cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2]
  center = None
  if len(cnts) > 0:
   c = max(cnts, key=cv2.contourArea)
   ((x, y), radius) = cv2.minEnclosingCircle(c)
   M = cv2.moments(c)
   center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
   #print(center)
   pub.publish(int(M["m01"] / M["m00"]))
   rospy.loginfo(int(M["m01"] / M["m00"]))
   rate.sleep()
   if radius > 10:
    cv2.circle(frame, (int(x), int(y)), int(radius),(0, 255, 255), 2)
    cv2.circle(frame, center, 5, (0, 0, 255), -1)
  pts.appendleft(center)
  for i in xrange(1, len(pts)):
   if pts[i - 1] is None or pts[i] is None:
    continue
   thickness = int(np.sqrt(64 / float(i + 1)) * 2.5)
   cv2.line(frame, pts[i - 1], pts[i], (0, 0, 255), thickness)

  cv2.imshow("Frame", frame)
  if cv2.waitKey(1) & 0xFF==ord('q'):
   break

 rospy.spin()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
 video_capture.release()
 cv2.destroyAllWindows()
 pass

หรือ ดาวน์โหลด ได้ที่

...

ไฟล์ : dc_motor.py

เขียนโค้ดเสร็จแล้ว Save As.. เก็บไฟล์ไว้ที่ โฟลเดอร์ robot_pkg/scripts

ต้องการให้เป็นไฟล์ที่ ROS เรียกใช้งานได้ โดยคำสั่ง:

cd catkin_ws/src/robot_pkg/scripts

chmod u+x dc_motor.py

ทดสอบการทำงาน

หน้าต่างที่1 เราจะ RUN ระบบ ROS จะต้องมี Core ที่ทำหน้าที่เป็นหัวใจของระบบ

roscore

หลังจากนั้นจึง Run Node ของเราได้

หน้าต่างที่ 2 เปิดการทำงาน x_sensor.py

rosrun robot_pkg x_sensor.py

หน้าต่างที่ 3 เปิดการทำงาน dc_motor.py

rosrun robot_pkg dc_motor.py

ผลลัพธ์การทำงาน แสดงการติดตามวัตถุ

..

...

03 พฤศจิกายน 2561

หุ่นยนต์ติดตามวัตถุ (ลูกปิงปอง) ใช้ ROS