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

2020.12.10

 

この記事は私が金沢工業大学ロボティクス学科で担当している2020年度後学期講義ロボットプログラミングⅡ用です。今回は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("/create1/camera/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のパッケージをインストールする。
  •  プログラムのダウンロード
    • $  cd  ~/src/robot_programming2
    • $  git pull
      • git pullでcv_bridge_tutorialをダウンロードできない場合は以下の作業を行う。
        • 学内
          • $ cd ~/src
          • $ rm ~/src/robot_programming2
          • $ git clone https://github.com/demulab/robot_programming2.git
        • 学外
          • $ cd ~/src
          • $ rm ~/src/robot_programming2
          • $ git clone https://github.com/demulab/robot_programming2.git -c http.proxy=””
        • ~/catkin_ws/srcの下にコピーする。
          • $ cp -r ~/src/robot_programming2/cv_bridge_tutorial  ~/catkin_ws/src
  • ビルド
    • $ cd ~/catkin_ws
    • $ catkin  build cv_bridge_tutorial
      • cv_bridgeパッケージでエラーが出る場合
        • ~/catkin_ws/src/vision_opencv/cv_bridgeディレクトリがあるか確認する。cv_bridgeはパッケージとしてインストールしているので、これは必要ないので以下のコマンドで~/tmpディレクトリを作り移動する。
          • $ mkdir ~/tmp
          • $ mv  ~/catkin_ws/src/vision_opencv/cv_bridge  ~/tmp


２. 実行

  • $ roslaunch ca_gazebo create_empty_world.launch
  • $ rosrun cv_bridge_tutorial cv_bridge_tutorial
  • 起動したgazeboの環境は何もないempty_worldなので、GazeboのInsertタブをクリックして、その中のCoke Canを選択する。ロボットの前方1mの位置をクリックするとそこに現れます。次のコマンドでロボットを動かしてカメラ画像が変わるのを確認しよう。
    • $ roslaunch ca_tools keyboard_teleop.launch

 

３．演習

  • シミュレータにコカ・コーラの缶(Coke Can)を挿入して、それを追うプログラムを作ろう
    • コーラ缶を挿入するためにはGazeboのInsertタブをクリックして、その中のCoke Canを選択して、Gazebo上の配置したい場所をクリックするとそこに現れます。
    • コーラ缶の位置を推定する方法はいろいろありますが、ここではマスクをかけた赤い画像の重心を求めることにします。OpenCVで画像の各画素にアクセスする方法は次のリンクを参考にしてください。

終わり

コメント

タイトルとURLをコピーしました