この記事は私が金沢工業大学ロボティクス学科で担当している2019年度後学期講義ロボットプログラミングⅡ用です。今回はcv_bridgeを使います。ROSでOpenCVを使いgazeboシミュレータのRGB-Dセンサから取得した画像から赤色の抽出とエッジ抽出を行います。
この記事は、以下のROSチュートリアルと「ROSで始めるロボットプログラミング、小倉著」を参考にしています。
ソース：ROSのチュートリアルと「ROSで始めるロボットプログラミング」をベースに改変しています。
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
static const std::string OPENCV_WINDOW = "Image window";
class ImageConverter
{
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub_;
image_transport::Publisher image_pub_;
public:
// コンストラクタ
ImageConverter()
: it_(nh_)
{
// カラー画像をサブスクライブ
image_sub_ = it_.subscribe("/camera/rgb/image_raw", 1,
&ImageConverter::imageCb, this);
// 処理した画像をパブリッシュ
image_pub_ = it_.advertise("/image_topic", 1);
}
// デストラクタ
~ImageConverter()
{
cv::destroyWindow(OPENCV_WINDOW);
}
// コールバック関数
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr, cv_ptr2, cv_ptr3;
try
{
// ROSからOpenCVの形式にtoCvCopy()で変換。cv_ptr->imageがcv::Matフォーマット。
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
cv_ptr3 = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat hsv_image, color_mask, gray_image, cv_image2, cv_image3;
// RGB表色系をHSV表色系へ変換して、hsv_imageに格納
cv::cvtColor(cv_ptr->image, hsv_image, CV_BGR2HSV);
// 色相(Hue), 彩度(Saturation), 明暗(Value, brightness)
// 指定した範囲の色でマスク画像color_mask(CV_8U:符号なし8ビット整数)を生成
// マスク画像は指定した範囲の色に該当する要素は255(8ビットすべて1)、それ以外は0
cv::inRange(hsv_image, cv::Scalar(150, 100, 50, 0) , cv::Scalar(180, 255, 255, 0), color_mask);
// ビット毎の論理積。マスク画像は指定した範囲以外は0で、指定範囲の要素は255なので、ビット毎の論理積を適用すると、指定した範囲の色に対応する要素はそのままで、他は0になる。
cv::bitwise_and(cv_ptr->image, cv_ptr->image, cv_image2, color_mask);
// グレースケールに変換
cv::cvtColor(cv_ptr->image, gray_image, CV_BGR2GRAY);
// エッジを検出するためにCannyアルゴリズムを適用
cv::Canny(gray_image, cv_ptr3->image, 15.0, 30.0, 3);
// ウインドウに円を描画
cv::circle(cv_ptr->image, cv::Point(100, 100), 20, CV_RGB(0,255,0));
// 画像サイズは縦横1/4に変更
cv::Mat cv_half_image, cv_half_image2, cv_half_image3;
cv::resize(cv_ptr->image, cv_half_image,cv::Size(),0.25,0.25);
cv::resize(cv_image2, cv_half_image2,cv::Size(),0.25,0.25);
cv::resize(cv_ptr3->image, cv_half_image3,cv::Size(),0.25,0.25);
// ウインドウ表示
cv::imshow("Original Image", cv_half_image);
cv::imshow("Result Image", cv_half_image2);
cv::imshow("Edge Image", cv_half_image3);
cv::waitKey(3);
// エッジ画像をパブリッシュ。OpenCVからROS形式にtoImageMsg()で変換。
image_pub_.publish(cv_ptr3->toImageMsg());
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_converter");
ImageConverter ic;
ros::spin();
return 0;
}
1. 準備
- opencv3のパッケージをインストールする。
- sudo apt update
- sudo apt upgrade
- sudo apt install libopencv-dev
- proxy環境下などでatpでエラーが出る場合はここを参照して設定する。
- 次に、サンプロプログラムをインストールしましょう。以下のファイルを~/catkin_ws/srcの下にコピーする。
- $ cd ~/catkin_ws/src
- $ tar xvf cv_bridge_tutorial.tar
- $ cd ~/catkin_ws
- $ catkin build
- RGB-Dセンサを使うのでTurtlebot3のモデルをwaffleに変更する。geditで~/.bashrcを開き、以下のようにburgerをwaffleに変更する。
- export TURTLEBOT3_MODEL=waffle
２. 実行
- $ roslaunch turtlebot3_gazebo turtlebot3_world.launch
- $ rosrun cv_bridge_tutorial cv_bridge_tutorial
- 次のコマンドでロボットを動かしてカメラ画像が変わるのを確認しよう
- $ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
３．演習
- シミュレータにコカ・コーラの缶(Coke Can)を挿入して、それを追うプログラムを作ろう
- コーラ缶を挿入するためにはGazeboのInsertタブをクリックして、その中のCoke Canを選択して、Gazebo上の配置したい場所をクリックするとそこに現れます。
- コーラ缶の位置を推定する方法はいろいろありますが、ここではマスクをかけた赤い画像の重心を求めることにします。OpenCVで画像の各画素にアクセスする方法は次のリンクを参考にしてください。
終わり
コメント