Backup diff of エージェント視点の画像取得(v120330, v1.4.8) vs current(No. 12)

The added line is THIS COLOR.
The deleted line is THIS COLOR.
Up:[[Tutorial]]　　 　　Previous：[[複数個のカメラ設置]]　　　　　Next：[[エージェントの衝突]]

#contents

*エージェント視点の画像取得 (captureView)[#g547d565]
ここではエージェント視線や視点を変化させその時の画像を取得するサンプルを紹介します。

※ここで使用するcaptureViewはSIGViewerのサービスプロバイダ機能を使用するため、ポートフォワーディングによる接続の場合は双方向のポートフォワーディングが必要になります。双方ポートフォワーディングの方法は[[こちら>ポートフォワーディングでサーバに接続する方法]]

**コントローラ作成 [#n54bccbb]
まずコントローラを作成します。
 $ cd ~/sigverse-<version>/bin/NewWorld
 $ emacs captureView.cpp

captureView.cpp

#highlight(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 onRecvMessage(RecvMessageEvent &evt);
 private:  
 
   //視線フラグ（視線が右に移動中か左に移動中か）
   bool move_eye; 
 
   //視線ベクトルの変化
   double dvvx; 
 
   //体を回転させるときの回転角
   int dy;
 
   //移動速度
   double vel;
 };
 
 void RobotController::onInit(InitEvent &evt)
 {
   //パラメータの設定  
   move_eye = false;
   dvvx     =  0.1;                                                                
   dy       = 45;                                                                      
   vel      = 10.0;
 }
 
 //定期的に呼び出される関数
 double RobotController::onAction(ActionEvent &evt)
 {
  
  SimObj *my = getObj(myname()); 
 
  //視線ベクトルのx成分を取得します。
  double vvx = my->evx1();
 
  double dvvx_tmp;
  //視線が右に移動中の場合
  if (move_eye == false)
    {
      dvvx_tmp = -1 * dvvx;
    }
    //左に移動中の場合                                                                                               
  //左に移動中の場合           
  else dvvx_tmp = dvvx;
 
  //視線を変化させます。
  vvx = vvx + dvvx_tmp;
  my->evx1(vvx);  
 
 
  //視線ベクトルのx成分を-1.0から1.0まで移動させます。
  if(vvx >= 1.0)
    {
      move_eye = false;
    }
  if( vvx <= -1.0)
    {
      move_eye = true;
    } 
 
  return 0.5;
 
 }
 
 //メッセージ受信時に呼び出される関数
 void RobotController::onRecvMessage(RecvMessageEvent &evt)
 {
  int n = evt.getSize();
  static int iImage = 0;
  if (n>0)
    {
       
      //取得したメッセージを表示します。
      std::string msg = evt.getString(0);
      LOG_MSG(("msg : %s", msg.c_str()));
      
  
      //メッセージ"capture"を受信するとcaptureViewを行います。
      if (strcmp(msg.c_str(), "capture") == 0)
	{
	  //エージェント視点の画像を取得します。
	  ViewImage *img_tmp = captureView(COLORBIT_ANY, IMAGE_320X240);
	  ViewImage *img = captureView(COLORBIT_ANY, IMAGE_320X240);
	  if (img)
	    {
	      //画像の大きさを表示します。
	      LOG_MSG(("(%d, %d)", img->getWidth(), img->getHeight()));
 	            
	      //Windows BMP 形式で保存します。
	      char fname[256];
	      sprintf(fname, "view%03d.bmp", iImage++);
	      img->saveAsWindowsBMP(fname);
 	            
	      //必要なくなったら削除します。
	      delete img_tmp;
	      delete img;
	    }
	}
       
      //メッセージ"rotation"を受信すると回転します。
      if (strcmp(msg.c_str(), "rotation") == 0){      
 
	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 (strcmp(msg.c_str(), "move") == 0){
	 

	SimObj *my = getObj(myname());
	

	//自分の位置を得ます。
	double x = my->x();
	double y = my->y();
	double z = my->z();
	
	//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( x + dx, y , z + dz );
	
      }
       
    }
 }
 
 extern "C" Controller * createController ()
 {
   return new RobotController;
 }
}}

-視線の変化 

このサンプルは定期的に呼び出される関数onActionで視線を常に変化させています。変化する視線の範囲は視線ベクトルのx成分が-1.0～1.0です。視線ベクトルのz成分が1, y成分が0の時エージェントの視線はまっすぐ前を向いた方向から左方向に45°、右方向に45°視線を行ったり来たりします。

-captureView 

エージェントが"capture"というメッセージを受信するとcaptureViewを実行しエージェント視点の画像を取得します。この時、実行時のディレクトリにview000.bmp, view001.bmpといったようにcaptureするたびに画像ファイルが保存されます。

※）このサンプルではcaptureViewを同時に2回行い2回目に得られた画像を保存しています。これはキャプチャしたときにOpenGLのダブルバッファリングの機能によりひとつ前にキャプチャした画像が得られてしまうためです。
//※）このサンプルではcaptureViewを同時に2回行い2回目に得られた画像を保存しています。これはキャプチャしたときにOpenGLのダブルバッファリングの機能によりひとつ前にキャプチャした画像が得られてしまうためです。

//最初にキャプチャしたときは原点から見た画像が取得されます。

-エージェントの移動 

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

**コンパイル [#m203eb4f]
それではコンパイルします。
 $ emacs Makefile

オブジェクトファイルの指定を修正します。
 #オブジェクトファイルの指定                                                                                        
 OBJS     = captureView.so

 $ make

**世界ファイル作成 [#u5216d9d]
 $ cd ..
 $ emacs xml/captureView.xml

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

captureView.xml
 <?xml version="1.0" encoding="utf8"?>
 <world name="VisTest2">
 
   <gravity x="0.0" y="-9.8" z="0.0"/> 
 
   <instanciate class="Robot-nii.xml"> 
	<set-attr-value name="name" value="robot_0"/>
	<set-attr-value name="dynamics" value="false"/>
 
	<set-attr-value name="x" value="0.0"/>
	<set-attr-value name="y" value="60.0"/>
	<set-attr-value name="z" value="-40.0"/>
 
        <set-attr-value name="epx1" value="0.0"/>
        <set-attr-value name="epy1" value="0.0"/>
        <set-attr-value name="epz1" value="5.0"/>
 
        <set-attr-value name="evx1" value="0.0"/>
        <set-attr-value name="evy1" value="0.0"/>
        <set-attr-value name="evz1" value="1.0"/>
 
	<set-attr-value name="language" value="c++"/>
	<set-attr-value name="implementation" value="./NewWorld/captureView.so"/>
#highlight(xml){{

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

  <gravity x="0.0" y="-9.8" z="0.0"/>

  <instanciate class="Robot-nii.xml">
    <set-attr-value name="name" value="robot_0"/>
    <set-attr-value name="dynamics" value="false"/>

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

    <set-attr-value name="epx1" value="0.0"/>
    <set-attr-value name="epy1" value="0.0"/>
    <set-attr-value name="epz1" value="5.0"/>

    <set-attr-value name="evx1" value="0.0"/>
    <set-attr-value name="evy1" value="0.0"/>
    <set-attr-value name="evz1" value="1.0"/>

    <set-attr-value name="language" value="c++"/>
    <set-attr-value name="implementation" value="./NewWorld/captureView.so"/>
  </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="82.0"/>
	<set-attr-value name="z" value="-250.0"/>
 
	<set-attr-value name="visStateAttrName" value="switch"/>
	<set-attr-value name="switch" value="on"/>
 
    <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="82.0"/>
    <set-attr-value name="z" value="-250.0"/>

    <set-attr-value name="visStateAttrName" value="switch"/>
    <set-attr-value name="switch" value="on"/>

  </instanciate>
 

  <instanciate class="seDoll_Bear.xml">
	<set-attr-value name="name" value="kuma_0"/>
	<set-attr-value name="dynamics" value="false"/>
 
	<set-attr-value name="x" value="0.0"/>
	<set-attr-value name="y" value="0.0"/>
	<set-attr-value name="z" value="0.0"/>
    <set-attr-value name="name" value="kuma_0"/>
    <set-attr-value name="dynamics" value="false"/>

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


  <instanciate class="seSofa_2seater.xml">
	<set-attr-value name="name" value="sofa_0"/>
	<set-attr-value name="dynamics" value="false"/>
 
	<set-attr-value name="x" value="-200.0"/>
	<set-attr-value name="y" value="20.0"/>
	<set-attr-value name="z" value="-100.0"/>
 
	<set-attr-value name="qw" value="0.707"/>
	<set-attr-value name="qx" value="0.0"/>
	<set-attr-value name="qy" value="0.707"/>
	<set-attr-value name="qz" value="0.0"/>
    <set-attr-value name="name" value="sofa_0"/>
    <set-attr-value name="dynamics" value="false"/>

    <set-attr-value name="x" value="-200.0"/>
    <set-attr-value name="y" value="20.0"/>
    <set-attr-value name="z" value="-100.0"/>

    <set-attr-value name="qw" value="0.707"/>
    <set-attr-value name="qx" value="0.0"/>
    <set-attr-value name="qy" value="0.707"/>
    <set-attr-value name="qz" value="0.0"/>
  </instanciate>
 

  <instanciate class="seTVbass_B.xml">
	<set-attr-value name="name" value="TVdai_0"/>
	<set-attr-value name="dynamics" value="false"/>
 
	<set-attr-value name="x" value="-20.0"/>
	<set-attr-value name="y" value="13.0"/>
	<set-attr-value name="z" value="-250.0"/>
    <set-attr-value name="name" value="TVdai_0"/>
    <set-attr-value name="dynamics" value="false"/>

    <set-attr-value name="x" value="-20.0"/>
    <set-attr-value name="y" value="13.0"/>
    <set-attr-value name="z" value="-250.0"/>
  </instanciate>
 

  <instanciate class="sePlant_B.xml">
	<set-attr-value name="name" value="ki_0"/>
	<set-attr-value name="dynamics" value="false"/>
 
	<set-attr-value name="x" value="100.0"/>
	<set-attr-value name="y" value="35.0"/>
	<set-attr-value name="z" value="-250.0"/>
    <set-attr-value name="name" value="ki_0"/>
    <set-attr-value name="dynamics" value="false"/>

    <set-attr-value name="x" value="100.0"/>
    <set-attr-value name="y" value="35.0"/>
    <set-attr-value name="z" value="-250.0"/>
  </instanciate>
 

  <instanciate class="seSidetable_B.xml">
	<set-attr-value name="name" value="sidetable_0"/>
	<set-attr-value name="dynamics" value="false"/>
 
	<set-attr-value name="x" value="-100.0"/>
	<set-attr-value name="y" value="23.0"/>
	<set-attr-value name="z" value="-100.0"/>
 
	<set-attr-value name="qw" value="0.707"/>
	<set-attr-value name="qx" value="0.0"/>
	<set-attr-value name="qy" value="0.707"/>
	<set-attr-value name="qz" value="0.0"/>
    <set-attr-value name="name" value="sidetable_0"/>
    <set-attr-value name="dynamics" value="false"/>

    <set-attr-value name="x" value="-100.0"/>
    <set-attr-value name="y" value="23.0"/>
    <set-attr-value name="z" value="-100.0"/>

    <set-attr-value name="qw" value="0.707"/>
    <set-attr-value name="qx" value="0.0"/>
    <set-attr-value name="qy" value="0.707"/>
    <set-attr-value name="qz" value="0.0"/>
  </instanciate>
 

  <instanciate class="seApple.xml">
	<set-attr-value name="name" value="apple_0"/>
	<set-attr-value name="dynamics" value="false"/>
 
	<set-attr-value name="x" value="-50.0"/>
	<set-attr-value name="y" value="10.0"/>
	<set-attr-value name="z" value="30.0"/>
    <set-attr-value name="name" value="apple_0"/>
    <set-attr-value name="dynamics" value="false"/>

    <set-attr-value name="x" value="-50.0"/>
    <set-attr-value name="y" value="10.0"/>
    <set-attr-value name="z" value="30.0"/>
  </instanciate>
 

  <instanciate class="seOrange.xml">
	<set-attr-value name="name" value="orange_0"/>
	<set-attr-value name="dynamics" value="false"/>
 
	<set-attr-value name="x" value="70.0"/>
	<set-attr-value name="y" value="10.0"/>
	<set-attr-value name="z" value="-30.0"/>
    <set-attr-value name="name" value="orange_0"/>
    <set-attr-value name="dynamics" value="false"/>

    <set-attr-value name="x" value="70.0"/>
    <set-attr-value name="y" value="10.0"/>
    <set-attr-value name="z" value="-30.0"/>
  </instanciate>
 

  <instanciate class="seOrange.xml">
	<set-attr-value name="name" value="orange_1"/>
	<set-attr-value name="dynamics" value="false"/>
 
	<set-attr-value name="x" value="80.0"/>
	<set-attr-value name="y" value="10.0"/>
	<set-attr-value name="z" value="-10.0"/>
    <set-attr-value name="name" value="orange_1"/>
    <set-attr-value name="dynamics" value="false"/>

    <set-attr-value name="x" value="80.0"/>
    <set-attr-value name="y" value="10.0"/>
    <set-attr-value name="z" value="-10.0"/>
  </instanciate>
 </world>

</world>
}}
**画像取得 [#j88e8a70]
それでは実行してみましょう
 $ ./sigserver.sh -p 9001 -w captureView.xml
 
SIGViewerで接続してみるとリビングルームにロボットが立っているのが見えます。
"start"ボタンを押してシミュレーションを開始して、ロボットをクリックすると左上の画像でエージェントの視線が動いているのがわかると思います。

次にSIGViewerからエージェントに"rotation"や"move"などのメッセージを送信してエージェントが向きを変えたり、移動したりすることを確認してみてください。

#ref(capture_1.jpg)

キャプチャしたいタイミングで"capture"を送信するとエージェント視線の画像が得られます。

#ref(view000.bmp)
*複数カメラの場合 [#odbc1548]
上のサンプルではエージェント設置されたカメラは一つでしたが、エージェントに複数個カメラが設置されている場合はcaptureViewするときにカメラ番号を指定することができます。

**世界ファイルの編集 [#z5a8d62b]
 $ emacs xml/captureView.xml
ヒューマノイドロボットにカメラ2を追加します。
ヒューマノイドロボットのカメラ設定の箇所を以下のように変更します。

captureView.xml

#highlight(xml:firstline[14]){{
    <set-attr-value name="epx1" value="0.0"/>
    <set-attr-value name="epy1" value="0.0"/>
    <set-attr-value name="epz1" value="5.0"/>
 
    <set-attr-value name="evx1" value="0.0"/>
    <set-attr-value name="evy1" value="0.0"/>
    <set-attr-value name="evz1" value="1.0"/>
}}
　　　　　　　　　　　↓
#highlight(xml:firstline[14]){{
    <set-attr-value name="epx1" value="-5.0"/>
    <set-attr-value name="epy1" value="0.0"/>
    <set-attr-value name="epz1" value="5.0"/>
 
    <set-attr-value name="evx1" value="0.18"/>
    <set-attr-value name="evy1" value="0.0"/>
    <set-attr-value name="evz1" value="1.0"/>
 
    <set-attr-value name="elnk2" value="HEAD_LINK"/>
 
    <set-attr-value name="epx2" value="5.0"/>
    <set-attr-value name="epy2" value="0.0"/>
    <set-attr-value name="epz2" value="5.0"/>
 
    <set-attr-value name="evx2" value="-0.18"/>
    <set-attr-value name="evy2" value="0.0"/>
    <set-attr-value name="evz2" value="1.0"/>
}}



 
カメラ１とカメラ２で微妙に位置と方向をずらしました。つまりカメラ１が左目、カメラ２が右目に対応します。

**コントローラ修正 [#l2655f2d]
次にカメラが2つある場合に対応したコントローラに修正します。

 $ cd NewWorld
 $ emacs captureView.cpp

#highlight(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 onRecvMessage(RecvMessageEvent &evt);
 private: 
 
   //視線フラグ（視線が右に移動中か左に移動中か）
   bool move_eye; 
 
   //視線ベクトルの変化
   double dvvx;
 
   //体を回転させるときの回転角
   int dy;
 
   //移動速度
   double vel;
 };
 
 void RobotController::onInit(InitEvent &evt)
 {
   //パラメータの設定
   move_eye = false;
   dvvx     =  0.1;
   dy       = 45;
   vel      = 10.0;
 }
 
 //定期的に呼び出される関数
 double RobotController::onAction(ActionEvent &evt)
 {
 
   SimObj *my = getObj(myname()); 
 
   //カメラ1の方向ベクトルのx成分を取得します。
   double vvx1 = my->evx1();
   //カメラ2の方向ベクトルのx成分を取得します。
   double vvx2 = my->evx2();
 
   double dvvx_tmp;
   //視線が右に移動中の場合
   if (move_eye == false)
     {
       dvvx_tmp = -1 * dvvx;
     }
   //左に移動中の場合
   else dvvx_tmp = dvvx;
 //視線を変化させます。
   vvx1 = vvx1 + dvvx_tmp;
   vvx2 = vvx2 + dvvx_tmp;
   my->evx1(vvx1);
   my->evx2(vvx2); 
 
 
   //視線ベクトルのx成分を-1.0から1.0まで移動させます。
   if((vvx1+vvx2)/2 >= 1.0)
     {
       move_eye = false;
     }
   if((vvx1+vvx2)/2 <= -1.0)
     {
       move_eye = true;
    }
 
   return 0.5;
 
 } 
 
 //メッセージ受信時に呼び出される関数
 void RobotController::onRecvMessage(RecvMessageEvent &evt)
 {
   int n = evt.getSize();
   static int iImage = 0;
   if (n>0)
     { 
 
       //取得したメッセージを表示します。
       std::string msg = evt.getString(0);
       LOG_MSG(("msg : %s", msg.c_str())); 
   
 
       //"capture"を含むメッセージを受信するとcaptureViewを行います。
       if (strstr(msg.c_str(), "capture") != NULL)
         {
           ViewImage *img_tmp;
           ViewImage *img;
           if(strstr(msg.c_str(),"1") != NULL)
             {
               //カメラ１の画像を取得します。
               img_tmp = captureView(COLORBIT_ANY, IMAGE_320X240, 1);
               img = captureView(COLORBIT_ANY, IMAGE_320X240, 1);
             }
           else if (strstr(msg.c_str(), "2") != NULL)
             {
               //カメラ２の画像を取得します。
               img_tmp = captureView(COLORBIT_ANY, IMAGE_320X240, 2);
               img = captureView(COLORBIT_ANY, IMAGE_320X240, 2);
             }
           else return; 
 
           if (img)
             {
               //画像の大きさを表示します。
               LOG_MSG(("(%d, %d)", img->getWidth(), img->getHeight()));  
 
              //Windows BMP 形式で保存します。
               char fname[256];
               sprintf(fname, "view%03d.bmp", iImage++);
               img->saveAsWindowsBMP(fname); 
 
               //必要なくなったら削除します。
               delete img_tmp;
               delete img;
             }
         } 
 
       //メッセージ"rotation"を受信すると回転します。
       if (strcmp(msg.c_str(), "rotation") == 0){ 
 
         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 (strcmp(msg.c_str(), "move") == 0){  
 
         SimObj *my = getObj(myname());  
 
         //自分の位置を得ます。 
         double x = my->x();
         double y = my->y();
         double z = my->z();  
 
         //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( x + dx, y , z + dz ); 
 
       }
     }
 }  
 
 extern "C" Controller * createController ()
  {
    return new RobotController;
  } 
}}
captureViewの最後の引数にカメラ番号を指定します。指定がなければ自動的にカメラ１の画像を取得します。このコントローラは受信したメッセージが"capture1"であればカメラ１、"capture2"であればカメラ２の画像を取得します。
**SIGViewerで表示 [#ve4e4156]
それではコンパイルして実行してみましょう。
 $ make
 $ cd ..
 $ ./sigserver.sh -p 9001 -w captureView.xml
SIGVerseを実行してSIGViewerで確認してみます。ロボットをクリックすると、SIGViewerの左上にカメラが2つついているのが確認できます。

#ref(capture_2kai.jpg)

ロボットに"capture1"または"capture2"を送ってそれぞれ別のカメラの画像が習得できることを確認してみてください。

#highlight(end)

Up:[[Tutorial]]　　　 　Previous：[[複数個のカメラ設置]]　　　　　Next：[[エージェントの衝突]]