基于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行宽的部分，在检测的图像中心绘制一个圆点用来标记。