Ros-Kinetic

1.環境準備

Ubuntu16.04
ROS-kinetic
opencv3.3.1
video-stream-opencv
一個USB攝像頭

video-stream-opencv是USB攝像頭驅動，關於它的介紹，請看github:https://github.com/ros-drivers/video_stream_opencv

安裝video-stream-opencv

cd ~/catkin_ws/src/
git clone https://github.com/ros-drivers/video_stream_opencv.git
cd ~/catkin_ws/
catkin_make

測試是否安裝成功

在終端輸入：

rosrun video_stream_opencv test_video_resource.py

如果能從攝像頭中看到影像說明安裝成功。

2.opencv格式的圖片與ROS中Image-message之間的相互轉換

ROS image消息類型到opencv類型的轉換：

cv_image = bridge.imgmsg_to_cv2(image_message, desired_encoding="passthrough")

Opencv類型到ROS Image類型的轉換：

image_message = cv2_to_imgmsg(cv_image, encoding="passthrough")

3.目標

接下來要完成使用Opencv獲取圖片，通過ROS的publisher（web_cam）發佈圖片到image_topic主題下，然後有一個新的listener（image-converter）訂閱這個topic,並在圖片中的制定位置畫出一個實心圓。

4.發佈者

在video_stream_opencv這個package下有一個test_video_resource.py,這個文件的 原始作用只是簡單的將OpenCv獲取到的圖片顯示出來，這裏我們要做的是通過這個node，將獲取到的圖片發佈到image_topic下去，以下是我修改過的 test_video_resource.py：

#! /usr/bin/env python2
# -*- coding: utf-8 -*-
"""

Copyright (c) 2015 PAL Robotics SL.
Released under the BSD License.

Created on 7/14/15

@author: Sammy Pfeiffer

test_video_resource.py contains
a testing code to see if opencv can open a video stream
useful to debug if video_stream does not work
"""

import sys
import signal
import cv2
import rospy
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image


def quit(signum, frame):
    print ''
    print 'stop fusion'
    sys.exit()


if __name__ == '__main__':
    signal.signal(signal.SIGINT, quit)
    signal.signal(signal.SIGTERM, quit)
    # 參數的長度
    if len(sys.argv) < 2:
        print "You must give an argument to open a video stream."
        print "  It can be a number as video device, e.g.: 0 would be /dev/video0"
        print "  It can be a url of a stream,        e.g.: rtsp://wowzaec2demo.streamlock.net/vod/mp4:BigBuckBunny_115k.mov"
        print "  It can be a video file,             e.g.: myvideo.mkv"
        exit(0)

    resource = sys.argv[1]
    # If we are given just a number, interpret it as a video device
    if len(resource) < 3:
        resource_name = "/dev/video" + resource
        resource = int(resource)
    else:
        resource_name = resource
    print "Trying to open resource: " + resource_name
    cap = cv2.VideoCapture(resource)
    if not cap.isOpened():
        print "Error opening resource: " + str(resource)
        print "Maybe opencv VideoCapture can't open it"
        exit(0)
    bridge = CvBridge()
    publisher = rospy.Publisher('image_topic', Image, queue_size=10)
    rospy.init_node('web_cam')
    print "Correctly opened resource, starting to show feed."
    rval, frame = cap.read()

    while rval:
        # 將opencv格式的圖片轉換爲ROS可接受的msg
        image_message = bridge.cv2_to_imgmsg(frame, encoding="bgr8")
        try:
            publisher.publish(image_message)
        except CvBridgeError as e:
            print(e)
        rval, frame = cap.read()

以上代碼不做過多解釋，都比較簡單。特別指出以下代碼是爲了堅挺鍵盤當用戶輸入Ctrl+c的時候節點終止

def quit(signum, frame):
    print ''
    print 'stop fusion'
    sys.exit()


if __name__ == '__main__':
    signal.signal(signal.SIGINT, quit)
    signal.signal(signal.SIGTERM, quit)
    ...

    while rval:
        ...

5.訂閱者

訂閱者的作用就是上邊說的從image-topic中訂閱消息，然後將其轉換爲opencv格式的圖片，然後在圖片中畫出一個實心圓，在與發佈者相同的文件夾下創建一個web_cam_listener.py:

#!/usr/bin/env python
import roslib

roslib.load_manifest('video_stream_opencv')
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError


class image_converter:

    def __init__(self):
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("image_topic", Image, self.callback)

    def callback(self, data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)

        (rows, cols, channels) = cv_image.shape
        if cols > 60 and rows > 60:
            cv2.circle(cv_image, (150, 150), 20, (30, 144, 255), -1)
        cv2.imshow("Image window", cv_image)
        cv2.waitKey(3)


if __name__ == '__main__':
    ic = image_converter()
    rospy.init_node('image_converter')
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")
    cv2.destroyAllWindows()

6.運行節點

• 啓動第一個終端輸入roscore;
• 啓動（publisher）第二個終端輸入rosrun video_stream_opencv test_video_resource.py 0；
  得到如下輸出：

xzchuang@Vostro:~/catkin_ws$ rosrun video_stream_opencv test_video_resource.py 0Trying to open resource: /dev/video0
Correctly opened resource, starting to show feed.

• 啓動（Subscriber）啓動第三個終端輸入：

rosrun video_stream_opencv web_cam_listener.py

這時候會在攝像頭輸入中畫一個實心圓（橙色），然後顯示出來，如圖：

• 啓動一個新的終端調用rqt-graph插件：rosrun rqt_graph rqt_graph