1. 程式人生 > >OpenCV完成ROS移動機器人尋跡運動

OpenCV完成ROS移動機器人尋跡運動

encoding raw 攝像頭 ima spi med msh 之間 font

安裝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行寬的部分,在檢測的圖像中心繪制一個圓點用來標記。

OpenCV完成ROS移動機器人尋跡運動