ROS講座80 compressedイメージをOpenCVで読む

環境

この記事は以下の環境で動いています。

項目	値
CPU	Core i5-8250U
Ubuntu	16.04
ROS	Kinetic
Gazebo	7.0.0
python	2.7.12
OpenCV	3.3.1-dev

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

これまでOpenCVで取り込む画像はimage_raw[sensor_msgs/Image]を使っていました。ただ伝送路の帯域の都合で圧縮画像を使いたいことがあります。image_transportのROSノードを使ってもよいのですが面倒なのでOpenCVで変換してみます。

ソースコード

cam_lecture/scripts/edge_filter_compressed.py

#!/usr/bin/env python
import rospy
import sys
import cv2
from sensor_msgs.msg import Image, CompressedImage, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
import numpy as np

class cvBridgeDemo:
    def __init__(self):
        self.node_name = "cv_bridge_demo_compressed"
        rospy.init_node(self.node_name)
        rospy.on_shutdown(self.cleanup)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("input_image", CompressedImage, self.image_callback, queue_size=1)
        self.image_pub = rospy.Publisher('output_image', Image, queue_size=1)

    def image_callback(self, ros_image_compressed):
        try:
            np_arr = np.fromstring(ros_image_compressed.data, np.uint8)
            input_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        except CvBridgeError, e:
            print e

        output_image = self.process_image(input_image)
        self.image_pub.publish(self.bridge.cv2_to_imgmsg(output_image, "mono8"))

        cv2.imshow(self.node_name, output_image)   
        cv2.waitKey(1)

    def process_image(self, frame):
        grey = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        grey = cv2.blur(grey, (7, 7))
        edges = cv2.Canny(grey, 15.0, 30.0)
        return edges

    def cleanup(self):
        cv2.destroyAllWindows()   

if __name__ == '__main__':
    cvBridgeDemo()
    rospy.spin()

edge_filter.pyとの違いを説明します。
self.image_sub = rospy.Subscriber("input_image", CompressedImage, self.image_callback, queue_size=1)、def image_callback(self, ros_image_compressed):のようにsubする型をImage->CompressedImageに変更します。
ROSメッセージからOpenCV形式への変換はnp_arr = np.fromstring(ros_image_compressed.data, np.uint8)、input_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)で行います。

以下が起動用のlaunchです。

cam_lecture/launch/sim_edge_filter_compressed.launch

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find cam_lecture)/urdf/camera_sim.urdf" />
  <arg name="rvizconfig" default="$(find cam_lecture)/rviz/camera_sim.rviz" />
  <param name="robot_description" type="str" textfile="$(arg model)"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find cam_lecture)/world/camera_sim.world" />
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model my_robot" />

  <node name="edge_filter_compressed" pkg="cam_lecture" type="edge_filter_compressed.py" >
    <remap from="input_image" to="/camera/image_raw/compressed" />
  </node>

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />   
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>

実行

roslaunch cam_lecture sim_edge_filter_compressed.launch 

参考

ROS wiki

目次ページへのリンク

ROS講座の目次へのリンク