基於ROS完成尋跡運動

安裝opencv功能包：

$ sudo apt-get install ros-indigo-version-opencv libopencv-dev python-opencv

 檢測指示線：

#!/usr/bin/env python
# coding=utf-8

import rospy
from sensor_msgs.msg import Image
import cv2, cv_bridge
import numpy
from geometry_msgs.msg import Twist


class Follower:
    def __init__(self):
        self.bridge = cv_bridge.CvBridge()
        cv2.namedWindow("window", 1)
        # 訂閱usb攝像頭
        self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback)
        # self.image_sub = rospy.Subscriber("cv_bridge_image", Image, self.image_callback)
        # 訂閱深度相機
        # self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback)
        # self.image_sub = rospy.Subscriber("/camera/depth/image_raw", Image, self.image_callback)
　　　　  
        self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
        self.twist = Twist()

    def image_callback(self, msg):
        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        # hsv將RGB圖像分解成色調H，飽和度S，明度V
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        # 顏色的范圍        # 第二個參數：lower指的是圖像中低於這個lower的值，圖像值變為0
        # 第三個參數：upper指的是圖像中高於這個upper的值，圖像值變為0
        # 而在lower～upper之間的值變成255
        lower_black = numpy.array([0, 0, 0])
        upper_black = numpy.array([50, 50, 50])
        mask = cv2.inRange(hsv, lower_black, upper_black)
        masked = cv2.bitwise_and(image, image, mask=mask)

        # 在圖像某處繪制一個指示，因為只考慮20行寬的圖像，所以使用numpy切片將以外的空間區域清空
        h, w, d = image.shape
        search_top = 3*h/4
        search_bot = search_top + 20
        mask[0:search_top, 0:w] = 0
        mask[search_bot:h, 0:w] = 0
        # 計算mask圖像的重心，即幾何中心
        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)
　　　　　　　　
            if not cv2.circle:
                self.twist.linear.x = -0.5
                self.twist.angular.z = 0.1
            # 計算圖像中心線和目標指示線中心的距離
            erro = cx - w/2
            self.twist.linear.x = 0.5
            # self.twist.angular.z = -float(erro)/100
            self.twist.angular.z = 0

            self.cmd_vel_pub.publish(self.twist)
        cv2.imshow("window", image)
        cv2.waitKey(3)


rospy.init_node("opencv")
follower = Follower()
rospy.spin()

　　使用opencv的cvtColor()函數將RGB圖像轉換成HSV圖像，使用numpy在HSV顏色空間中創建一個所需的色調范圍，然后用inRange()函數根據色調范圍生成一個二值圖像。

　　跟蹤指示線的策略：只考慮圖像1/3高處的20行寬的部分，在檢測的圖像中心繪制一個圓點用來標記。