ROS x ビデオカメラで遊んでみる
やりたいこと
ROSでUSBカメラを使ってみる
教科書
ライセンス
<maintainer email="fei@kanto-gakuin.ac.jp">Fei Qian</maintainer> <license>BSD</license> <author>Fei Qian</author>
環境設定
sudo apto-get install ros-kinetic-usb-cam
sudo apt-get install ros-kinetico-image-pipeline
設定
usb_camについて詳細： usb_cam - ROS Wiki
pixel_format
RGB データと YUV データについて: YUVは、Yは輝度、Uは輝度と青の差、Vは輝度と赤の差
👆超わかりやすい
コーデックについて：
フォーマットの設定と受信、確認
$ 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.
カメラキャリブレーション
参考： カメラ キャリブレーションとは - MATLAB & Simulink - MathWorks 日本
チェッカーボードをダウンロード
-
rosrun usb_cam usb_cam_node
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.026 image:=/usb_cam/image_raw
キャリブデータの抽出
ビデオストリームの配布と購読
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>で使用可能
参考： http://mprg-robot.isc.chubu.ac.jp/image_transport/
image_transportでトピック配布
以下の２つのメソッドを提供する
ImageTransportのadvertise
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が提供しているプラグイン
image_transport ("raw") - The default transport, sending sensor_msgs/Image through ROS.
compressed_image_transport ("compressed") - JPEG or PNG image compression.
theora_image_transport ("theora") - Streaming video using the Theora codec.
参考：
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】 | AviUtlの易しい使い方
参考：
cv_bridge
ROSではOpenCVを簡単に使えるようなインターフェースcv_bridgeを提供している。
- #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>
$ 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>
$ 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
