🕌

choreonoidで画像取得する

2023/05/12に公開

OpenCV

choreonoid

tech

やりたいこと

Choreonoidのロボットモデルに搭載したカメラからデータ取得して画像表示したい。

ロボットモデルの修正

カメラアイテムは以下のようにして対象のBodyファイルに追記。詳しくはこちらのドキュメントを参照。

-
    name: cam_link
    parent: base
    translation: [ 0.01425, -0.019, 0.04975 ]
    rotation: [ [ 1, 0, 0, 90 ], [ 0, 1, 0, -90 ] ]
    jointType: fixed
    elements:
      -
        type: Camera
        name: camera
        format: COLOR
        fieldOfView: 58
        nearClipDistance: 0.001
        width: 640
        height: 360
        frameRate: 30
        on: true

コントローラーからカメラ画像の取得

今回はSimpleControllerをベースにカメラ画像を取得するコードを作成する。以下が実装例。デモとして、取得した画像をグレースケールへ変換しCannyアルゴリズムによってエッジ検出を行っている。

#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/opencv.hpp"

#include <cnoid/Camera>
#include <cnoid/SimpleController>

class VisionController : public cnoid::SimpleController
{
public:
  VisionController() : sampling_time_(0.1) {}
  ~VisionController() = default;

  virtual bool initialize(cnoid::SimpleControllerIO * io) override
  {
    io_ = io;
    camera_ptr_ = io->body()->findDevice<cnoid::Camera>("camera");
    io->enableInput(camera_ptr_);
    return true;
  }

  virtual bool control() override
  {
    double time = io_->currentTime();

    if (sampling_time_ < std::fabs(time - previous_time_)) {
      const cnoid::Image & image_raw = camera_ptr_->constImage();

      const int height = image_raw.height();
      const int width = image_raw.width();

      cv::Mat sensor_image(height, width, CV_8UC3, const_cast<uchar *>(&image_raw.pixels()[0]));

      cv::Mat gray_image, canny_image;
      cvtColor(sensor_image, gray_image, cv::COLOR_RGB2GRAY);
      Canny(gray_image, canny_image, 50, 200);

      cv::imshow("vision", canny_image);

      previous_time_ = time;
    }

    return true;
  }

private:
  cnoid::SimpleControllerIO * io_;
  cnoid::CameraPtr camera_ptr_;
  double previous_time_{0.0};
  double sampling_time_;
};

CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(VisionController)

実行

楽しそう

コードの解説

camera_ptr_ = io->body()->findDevice<cnoid::Camera>("camera");

対象のロボットのカメラアイテムを取得する。引数にはBodyファイルに追記したカメラデバイスの名前を指定する。

const cnoid::Image & image_raw = camera_ptr_->constImage();

Imageデータを取得する。cnod::Imageクラスはpixel情報(vectorでunsigned charが格納)、height、width、エンコーディング情報を取得することができる。これをOpenCVのcv::Matで使えるようにする。

const int height = image_raw.height();
const int width = image_raw.width();

cv::Mat sensor_image(height, width, CV_8UC3, const_cast<uchar *>(&image_raw.pixels()[0]));

ここまでくれはあとはOpenCVの画像処理アルゴリズムを色々適用できるようになる。
ROS環境でsensor_msgsをpublishしたい場合はROSのプラグインを使うのがオススメ。

やりたいこと

ロボットモデルの修正

コントローラーからカメラ画像の取得

実行

コードの解説

Discussion