空飛ぶロボットのつくりかた

ロボットをつくるために必要な技術をまとめます。ロボットの未来についても考えたりします。

ROS x ビデオカメラで遊んでみる

やりたいこと

ROSでUSBカメラを使ってみる

www.logicool.co.jp

教科書

ROSプログラミング | 森北出版株式会社

ライセンス

  <maintainer email="fei@kanto-gakuin.ac.jp">Fei Qian</maintainer>
  <license>BSD</license>
  <author>Fei Qian</author>

環境設定

  1. sudo apto-get install ros-kinetic-usb-cam

  2. sudo apt-get install ros-kinetico-image-pipeline

設定

usb_camについて詳細: usb_cam - ROS Wiki

pixel_format

RGB データと YUV データについて:  YUVは、Yは輝度、Uは輝度と青の差、Vは輝度と赤の差

  1. http://www.eizo.co.jp/support/db/files/technical_information/2010/WP10-009.pdf

👆超わかりやすい

コーデックについて:

  1. 動画コーデックの種類と違い(H.264・VP9・MPEG・Xvid・DivX・WMV等)【比較】

  2. 色空間:RGBとYUVの違い - ぼくんちのTV 別館

フォーマットの設定と受信、確認

$ rosparam set /usb_cam/pixel_format mjpeg

$ rosrun usb_cam usb_cam_node &

$ rosrun image_view image_view image:=/usb_cam/image_raw

この他に

$ rqtのpluginsのvisualizationから見ることもできる

$ rqt_image_viewでも見れる

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video0" />
    <param name="camera_frame_id" value="usb_cam" />
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen" required="true">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

[ WARN] [1519217872.252076062]: Camera calibration file /home/takuya-usb/.ros/camera_info/head_camera.yaml not found.

カメラキャリブレーション

f:id:robonchu:20180126150406p:plain

参考: カメラ キャリブレーションとは - MATLAB & Simulink - MathWorks 日本

f:id:robonchu:20180221221301p:plain

  1. チェッカーボードをダウンロード

  2. キャリブレーション

    • rosrun usb_cam usb_cam_node

    • rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.026 image:=/usb_cam/image_raw

  3. キャリブデータの抽出

    1. cd /tmp

    2. tar zxvf calibrationdata.tar.gz

    3. cp ost.txt ost.ini

    4. rosrun camera_calibration_parsers convert ost.ini head_camera.yaml

    5. head_camera.yamlのcamera_nameをhead_cameraに変更

    6. head_camera.yamlを~/.ros/camera.infoにコピー

ビデオストリームの配布と購読

参考:image_transport - ROS Wiki

image_transport

image transportを用いると、イメージを圧縮して送ることにより、ビデオストリームを転送する際の帯域への圧迫を緩和することができる。

  • pythonはimage_transportをサポートしていないため、以下のようにrepublishを用いる。

    • $ republish [in_transport] in:=<in_base_topic> [out_transport] out:=<out_base_topic>
  • cppでは#include <image_transport/image_transport.h>で使用可能

参考: 【ROS】画像圧縮によってフレームレートを向上させるパッケージの紹介 – MPRG Robot Think Tanks

image_transportでトピック配布

以下の2つのメソッドを提供する

  1. ImageTransportのadvertise

  2. ImageTransportのadvertiseCamera

  • image_transportのpublisherは単一トピック配布とは異なり、複数の独立したトピックを同時に配布することができる。

  • compressedで別のトピックで配布するなら、トピック名は<ベーストピック名>/compressedとなる。

  • このcompressedのような補助機能をサブトピックという。

$rosmsg show sensor_msgs/Image

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data
image_transportが提供しているプラグイン
  1. image_transport ("raw") - The default transport, sending sensor_msgs/Image through ROS.

  2. compressed_image_transport ("compressed") - JPEG or PNG image compression.

  3. theora_image_transport ("theora") - Streaming video using the Theora codec.

  • CameraPublisherでは上記トピックに加えて、base_topic/camera_infoも利用することができる

  • Theora(セオラ)とは: オープンな非可逆の動画圧縮ファイルフォーマット、または、そのコーデックである。

  • PNGとは: Portable Network Graphics

  • JPEGとは: Joint Photographic Experts Group

参考:

Theoraコーデックを使ってみた① [インストール] - It’s now or never

どうやって使い分けるべきなのか。JPG・PNG・GIFの違いをまとめてみました。 | ギズモード・ジャパン

image_transportの使い方

image_transport/Tutorials/WritingNewTransport - ROS Wiki

購読する時はベーストピック名を使用する。

サブトピック名は以下のようにパラメータでセットする。

  • rosparam set base_topic/image_transport [raw | compressed | theora]

キーフレームとは: キーフレームとは?Iフレーム・Pフレーム・Bフレームの違い【GOP】

参考:

camera_calibration - ROS Wiki

cv_bridge

ROSではOpenCVを簡単に使えるようなインターフェースcv_bridgeを提供している。

  • ROSのストリームタイプ: sensor_msgs/Image

  • OpenCVのタイプ: Intel image processing library

f:id:robonchu:20180126154843p:plain

  • #include <cv_bridge/cv_bridge.h>とすれば使用可能。

イメージ購読プログラム

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>

//#include <opencv2/highgui/highgui.hpp>


void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    try {
        cv::imshow("Image_Subscriber", cv_bridge::toCvShare(msg, "bgr8")->image);
        cv::waitKey(30);
    }
    catch (cv_bridge::Exception& e) {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "image_subscriber");
    ros::NodeHandle nh;
    cv::namedWindow("Image_Subscriber");
    cv::startWindowThread();

    image_transport::ImageTransport it(nh);
    image_transport::Subscriber sub = it.subscribe("camera_raw", 1, imageCallback);

    ros::spin();
    cv::destroyWindow("camera_view");

    return EXIT_SUCCESS;
}

CMake

参考: ja/catkin/CMakeLists.txt - ROS Wiki

cmake_minimum_required(VERSION 2.8.3)
project(my_camera)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  cv_bridge
  image_transport
)

catkin_package(
  CATKIN_DEPENDS roscpp std_msgs cv_bridge image_transport
)

find_package(OpenCV)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

add_executable(image_sub src/image_subscriber.cpp)
target_link_libraries(image_sub
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
)

参考: ja/catkin/CMakeLists.txt - ROS Wiki

pakage.xml

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>image_transport</build_depend>
  <build_depend>cv_bridge</build_depend>

  <run_depend>roscpp</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>image_transport</run_depend>
  <run_depend>cv_bridge</run_depend>

イメージ配布プログラム

以下でカメラの各パラメータをチェック

  • $ v4l2-ctl -d /dev/video0 --list-formats-ext
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/highgui/highgui.hpp>

int main( int argc, char **argv )
{
    ros::init( argc, argv, "image_publisher" );
    ros::NodeHandle nh;

    // Open camera with CAMERA_INDEX (webcam is typically #0).
    const int CAMERA_INDEX = 0;
    cv::VideoCapture capture( CAMERA_INDEX );
    if( not capture.isOpened() ) {
        ROS_INFO_STREAM("Failed to open camera with index " << CAMERA_INDEX << "!");
        ros::shutdown();
    }
    // Set the parameters using the results of executing the following command:
    //   v4l2-ctl -d /dev/video0 --list-formats-ext
    capture.set(CV_CAP_PROP_FRAME_WIDTH, 640);
        capture.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
        capture.set(CV_CAP_PROP_FPS, 30);
    //double width = capture.get(CV_CAP_PROP_FRAME_WIDTH);
    //double height = capture.get(CV_CAP_PROP_FRAME_HEIGHT);
    //ROS_DEBUG_STREAM("width: " << width << ", height: " << height);

    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub_image = it.advertise( "camera_raw", 1 );

    // for debug
    cv::namedWindow("Image_Publisher", CV_WINDOW_AUTOSIZE | CV_WINDOW_FREERATIO);

    cv_bridge::CvImagePtr frame = boost::make_shared< cv_bridge::CvImage >();
    frame->encoding = sensor_msgs::image_encodings::BGR8;
    while( ros::ok() ) {
        // for debug
        cv::Mat image;
        capture >> image;
        cv::imshow("Image_Publisher", image);
        //

        capture >> frame->image;
        if( frame->image.empty() ) {
            ROS_ERROR_STREAM( "Failed to capture frame!" );
            ros::shutdown();
        }
        frame->header.stamp = ros::Time::now();
        pub_image.publish( frame->toImageMsg() );

        if(cv::waitKey(30) >= 0) break;
        ros::spinOnce();
    }

    capture.release();
    return EXIT_SUCCESS;
}

launch

<launch>
  <param name="pixel_format" value="mjpeg" />
  <node name="image_publisher"  pkg="my_camera" type="image_pub"  output="screen"/>
  <node name="image_subscriber" pkg="my_camera" type="image_sub" output="screen"/>
</launch>

f:id:robonchu:20180221230211p:plain

$ rostopic list

/camera_raw
/camera_raw/compressed
/camera_raw/compressed/parameter_descriptions
/camera_raw/compressed/parameter_updates
/camera_raw/compressedDepth
/camera_raw/compressedDepth/parameter_descriptions
/camera_raw/compressedDepth/parameter_updates
/camera_raw/theora
/camera_raw/theora/parameter_descriptions
/camera_raw/theora/parameter_updates
/rosout
/rosout_agg

カメライメージ配布プログラム

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/highgui/highgui.hpp>

int main( int argc, char **argv )
{
    ros::init( argc, argv, "image_Camerapublisher" );
    ros::NodeHandle n;

    // Open camera with CAMERA_INDEX (webcam is typically #0).
    const int CAMERA_INDEX = 0;
    cv::VideoCapture capture( CAMERA_INDEX );
    if( not capture.isOpened() ) {
        ROS_INFO_STREAM("Failed to open camera with index " << CAMERA_INDEX << "!");
        ros::shutdown();
    }

    // Set the parameters using the results of executing the following command:
    //   v4l2-ctl -d /dev/video0 --list-formats-ext
    capture.set(CV_CAP_PROP_FRAME_WIDTH, 640);
        capture.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
        capture.set(CV_CAP_PROP_FPS, 30);

    image_transport::ImageTransport it( n );
    image_transport::CameraPublisher pub_image = it.advertiseCamera( "camera_raw", 1 );
    sensor_msgs::CameraInfo cam_info;

    // for debug
    //cv::namedWindow("Image_CameraPublisher", CV_WINDOW_AUTOSIZE | CV_WINDOW_FREERATIO);

    cv_bridge::CvImagePtr frame = boost::make_shared< cv_bridge::CvImage >();
    frame->encoding = sensor_msgs::image_encodings::BGR8;
    while( ros::ok() ) {
        // for debug
        //cv::Mat image;
        //capture >> image;
        //cv::imshow("Image_CameraPublisher", image);
        //

        capture >> frame->image;
        if( frame->image.empty() ) {
            ROS_ERROR_STREAM( "Failed to capture frame!" );
            ros::shutdown();
        }
        frame->header.stamp = ros::Time::now();
        cam_info.header.stamp = frame->header.stamp;
        pub_image.publish( frame->toImageMsg() , 
            sensor_msgs::CameraInfoConstPtr(new sensor_msgs::CameraInfo(cam_info)));

        if(cv::waitKey(30) >= 0) break;
        ros::spinOnce();
    }

    capture.release();
    return EXIT_SUCCESS;
}

カメライメージ購読プログラム

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info)
{
    try {
        cv::imshow("image_CameraSubscriber", cv_bridge::toCvShare(msg, "bgr8")->image);
        if(cv::waitKey(30) >= 0) 
            ros::shutdown();
    }
    catch (cv_bridge::Exception& e) {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "image_CameraSubscriber");
    ros::NodeHandle nh;
    cv::namedWindow("image_CameraSubscriber");
    cv::startWindowThread();

    image_transport::ImageTransport it(nh);
    image_transport::CameraSubscriber sub;

    sub = it.subscribeCamera("camera_raw", 1, imageCallback);

    ros::spin();
    cv::destroyWindow("image_CameraSubscriber");

    return EXIT_SUCCESS;
}

launch

<launch>
  <node name="camera_publisher"  pkg="my_camera" type="camera_pub"  output="screen"/>
  <node name="camera_subscriber" pkg="my_camera" type="camera_sub" 
    required="true" output="screen"/>
</launch>

f:id:robonchu:20180221231418p:plain

$ rostopic list

/camera_info
/camera_raw
/camera_raw/compressed
/camera_raw/compressed/parameter_descriptions
/camera_raw/compressed/parameter_updates
/camera_raw/compressedDepth
/camera_raw/compressedDepth/parameter_descriptions
/camera_raw/compressedDepth/parameter_updates
/camera_raw/theora
/camera_raw/theora/parameter_descriptions
/camera_raw/theora/parameter_updates
/rosout
/rosout_agg