エージェント視点の画像取得(v2.0系)

Up:Tutorial?　　　　Previous：サービスプロバイダの使用方法　　　　　Next：エージェントの衝突

エージェント視点の画像取得 (captureView)
Old Version

エージェント視点の画像取得 (captureView) †

ここではサービスプロバイダ機能を使用しエージェントに設置されたカメラ画像のデータをコントローラが取得するサンプルを紹介します。

※SSH接続によりSIGVerseを利用している場合は前ページ同様追加でポートフォワーディングの設定が必要です。詳しくは前ページサービスプロバイダの使用方法を参照ください。

コントローラ作成 †

まずコントローラを作成します。

$ cd ~/sigverse-<version>/bin/NewWorld
$ emacs captureView.cpp

captureView.cpp

#include <Controller.h>
#include <ControllerEvent.h>
#include <Logger.h>
#include <ViewImage.h>
#include <math.h>

#define PI 3.141592
#define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 )

class RobotController : public Controller
{
public:
  void onInit(InitEvent &evt);
  double onAction(ActionEvent &evt);
  void onRecvMsg(RecvMsgEvent &evt);

private:
  ViewService* m_view;
  double vel;
};

void RobotController::onInit(InitEvent &evt)
{
  // 移動速度
  vel = 10.0;
  // サービスに接続
  m_view = (ViewService*)connectToService("SIGViewer", 9005);
}

//定期的に呼び出される関数
double RobotController::onAction(ActionEvent &evt)
{
  return 10.0;
}

//メッセージ受信時に呼び出される関数
void RobotController::onRecvMsg(RecvMsgEvent &evt)
{
  static int iImage = 0;

//取得したメッセージを表示します
  std::string msg = evt.getMsg();
  LOG_MSG(("msg : %s", msg.c_str()));

//メッセージ"capture"を受信するとcaptureViewを行います
  if (msg == "capture") {
    if(m_view != NULL) {

// ビット深度24,画像サイズ320X240の画像を取得します
      ViewImage *img = m_view->captureView(2, COLORBIT_24, IMAGE_320X240);
      if (img != NULL) {

// 画像データを取得します
        char *buf = img->getBuffer();

//Windows BMP 形式で保存します
        char fname[256];
        sprintf(fname, "view%03d.bmp", iImage++);
        img->saveAsWindowsBMP(fname);

//必要なくなったら削除します
        delete img;
      }
    }
  }

//メッセージ"rotation"を受信すると回転します
  if (msg == "rotation"){

SimObj *my = getObj(myname());

// 自分のy軸周りの回転を得ます(クオータニオン)
    double qy = my->qy();

//くオータニオンから回転角(ラジアン)を導出します
    double theta = 2*asin(qy);

//体全体を回転させます
    double y = theta + DEG2RAD(45);
    if( y >= PI)
      y = y - 2 * PI;

my->setAxisAndAngle(0, 1.0, 0, y);
   }

//メッセージ"move"を受信すると自分の向いている方向に進みます
  if (msg == "move"){

SimObj *my = getObj(myname());

//自分の位置を得ます
    Vector3d pos;
    my->getPosition(pos);

//y軸周りの自分の回転を得ます（クオータニオン）
    double qy = my->qy();

//クオータニオンから回転角を導出します
    double theta = 2*asin(qy);

//移動距離
    double dx = 0.0;
    double dz = 0.0;

//移動する方向を決定します
    dx = sin(theta) * vel;
    dz = cos(theta) * vel;

//移動します
    my->setPosition( pos.x() + dx, pos.y() , pos.z() + dz );
  }
}

extern "C" Controller * createController ()
{
  return new RobotController;
}

↑

captureView †

50行目の関数captureViewで画像データをビューワーから取得します。

1番目の引数は取得したいカメラのID番号を指定します。ここではID番号2を指定しました。2番目と3番目に1ピクセルあたりのビット深度と画像サイズを指定します。(※v121029, v2.0.1ではビット深度24, 画像サイズ320×240のみ取得可）

取得したViewImageから画像データを取り出すには関数getBuffer()を用います。

このサンプルではエージェントに"capture"というメッセージを送信するとエージェントは画像を取得することができます。

captureViewが成功すると実行ディレクトリにview000.bmp, view001.bmp...というようにbmp形式の画像ファイルが保存されます。

↑

エージェントの移動 †

このエージェントはメッセージ"move"を受信すると現在向いている方向に進みます。"rotation"を受信するとエージェントは向きを変えます。

↑

コンパイル †

Makefileの変数OBJSにcaptureView.soを追加してコンパイルします。

↑

世界ファイル作成 †

$ cd ..
$ emacs xml/captureView.xml

世界ファイルは前ページ視覚に関する操作で使用したリビングルームの世界ファイルとほとんど同じものを使います。

captureView.xml

<?xml version="1.0" encoding="utf8"?>
<world name="myworld5">

<instanciate class="Robot-nii.xml">

<set-attr-value name="name" value="robot_000"/>

<set-attr-value name="language" value="c++"/>

<set-attr-value name="implementation"
                    value="./NewWorld/captureView.so"/>

<set-attr-value name="dynamics" value="false"/>

<set-attr-value name="x" value="0.0"/>
    <set-attr-value name="y" value="54.0"/>
    <set-attr-value name="z" value="-40.0"/>

<camera id="1"
            link="HEAD_LINK"
            direction="0.1 0 1"
            position="-2.0 0.0 7.0"/>
    <camera id="2"
            link="HEAD_LINK"
            direction="-0.1 0 1"
            position="2.0 0.0 7.0"/>
  </instanciate>

<instanciate class="seTV.xml">
    <set-attr-value name="name" value="TV_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-20.0"/>
    <set-attr-value name="y" value="62.0"/>
    <set-attr-value name="z" value="-250.0"/>
    <set-attr-value name="visStateAttrName" value="switch"/>
    <set-attr-value name="switch" value="on"/>
  </instanciate>

</world>

エージェントに2つのカメラを設置し、2つのカメラで微妙に位置と方向をずらしました。

カメラID番号1が右目、2が左目に相当します。

↑

画像取得 †

それでは実行してみましょう

$ ./sigserver.sh -p 9001 -w captureView.xml

SIGViewerで接続してみるとリビングルームにロボットが立っているのが見えます。

シミュレーションを開始して"rotation"や"move"などのメッセージを送信するとエージェントが向きを変えたり、移動したりして自由に部屋の中を移動できるようになります。

キャプチャしたいタイミングで"capture"を送信するとエージェントに設置されたカメラ(ID番号2）で撮影した画像が得られます。

サーバ側の実行ディレクトリにカメラ2でcaptureした画像ファイルが保存されていれば成功です。

↑

Old Version †

エージェント視点の画像取得(v120330, v1.4.8)