ROS演習9-2018: ロボットビジョン （OpenCVとの連携）

この記事は私が金沢工業大学ロボティクス学科で担当している講義ロボットプログラミングⅡ用です。今回はcv_bridgeを使います。ROSでOpenCVを使いgazeboシミュレータのRGB-Dセンサから取得した画像から赤色の抽出とエッジ抽出を行います。

この記事は、以下のROSチュートリアルと「ROSで始めるロボットプログラミング、小倉著」を参考にしています。

• Converting between ROS images and OpenCV images (C++)

ソース：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 install ros-kinetic-opecv3
  • sudo apt update
  • sudo apt upgrade
•  次に、サンプロプログラムをインストールしましょう。以下のファイルを~/catkin_ws/srcの下にコピーする。
  • cv_bridge_tutorial180121
• $ cd ~/catkin_ws/src
• $ tar xvf cv_bridge_tutorial.tar
• $ cd ~/catkin_ws
• $ catkin_make
• RGB-Dセンサを使うのでTurtlebot3のモデルをwaffleに変更する。geditで~/.bashrcを開き、以下のようにburgerをwaffleに変更する。
  • export TURTLEBOT3_MODEL=waffle

2.  Gazeboバージョンアップ

• (1) gazebo7.0.0はバグがあり、カメラを使おうとすると落ちます。端末で次のコマンドを実行しgazeboのバージョンを調べる。
• (2) gazebo -v
• (3) gazeboバージョンが7.0.0の方は以下の方法でgazebo7.14.0にバージョンアップしましょう。なお、ros kineticはgazebo7を推奨しているのでgazebo8にはバージョンアップしないでください。
  • sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
  • wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
  • sudo apt update
  • sudo apt install gazebo7
•  gazebo -v　コマンドでgazebo7.14.0になっていれば成功です。

3. 実行

• $ 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で画像の各画素にアクセスする方法は次のリンクを参考にしてください。
    • RobotVision勉強会：第３回 カメラ・動画の読込みと画素へのアクセス

終わり