複数個のカメラ設置

Up:Tutorial?　　　　Previous：視覚に関する操作　　　　　Next：エージェント視点の画像取得?

複数個のカメラを設置する方法
- サブカメラを設置する
- メインビューの切り替え

複数個のカメラを設置する方法 †

ヒューマノイド型のエージェントはデフォルトでは顔に一つだけカメラが設置されていますが、複数個のカメラを体の好きな場所に取り付けることができます。ここではエージェントに複数個のカメラを設置する方法を説明します。なお、ヒューマノイド(関節を持ったエージェント）以外のエージェントはカメラを増やすことはできません。（SIGViewer1.3.5以降のバージョン対応）

↑

サブカメラを設置する †

エージェントにサブカメラを設置するサンプルコードを紹介します。

↑

コントローラ作成 †

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

SetCamera.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 )
 
 using namespace std; 
 
 class RobotController : public Controller
 { 
 public:
   void onInit(InitEvent &evt);
   double onAction(ActionEvent &evt);
   void onRecvMessage(RecvMessageEvent &evt); 
 
 private:
 };
 
 void RobotController::onInit(InitEvent &evt)
 {
 } 
 
 double RobotController::onAction(ActionEvent &evt)
 {
   return 0.1;
 } 
 
 void RobotController::onRecvMessage(RecvMessageEvent &evt)
 {
   int size = evt.getSize();
   if (size>0)
     {
       
       //取得したメッセージを表示します。
       string msg = evt.getString(0);
       LOG_MSG(("msg : %s", msg.c_str()));
       SimObj *my = getObj(myname());
       
       //=が含まれるメッセージを受信すると関節を曲げます。
       if(strstr(msg.c_str(),"=")) 
 	{
  
 	  //メッセージをJoint名と角度に分けます。   
 	  string msg_j;
 	  string angle_str;
 	  int n = 0; 
 	  n = msg.find("=");
 	  msg_j = msg.substr(0,n);
 	  angle_str = msg.substr(n+1);
 
 	  //受け取ったJoint名と角度を表示します。
 	  LOG_MSG(("Joint Name : %s", msg_j.c_str()));
 	  LOG_MSG(("Joint Angle : %s", angle_str.c_str()));
 
 	  //Jointを回転させます。
 	  double angle = atof(angle_str.c_str());
 	  my->setJointAngle(msg_j.c_str(), DEG2RAD(angle));
 
 	}
      
        //メッセージ"rotation"を受信するとエージェントの体全体が回転します。
        if (strcmp(msg.c_str(), "rotation") == 0){      
 	 
 	//回転角を設定します。
 	int dy = 45;
 	
 	// 自分の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){
 		
 	//自分の位置を得ます。
 	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;
 	
 	//移動する速度を設定します。
 	double vel = 10;
 	
 	//移動する方向を決定します。
 	dx = sin(theta) * vel;
 	dz = cos(theta) * vel;
 	
 	//移動します。
 	my->setPosition( x + dx, y , z + dz );
       }
    }
 } 
 
 extern "C" Controller * createController ()
 {
   return new RobotController;
 }

このエージェントはメッセージ"move"を受信すると現在向いている方向に進みます。"rotation"を受信するとエージェントは向きを変えます。回転する角度をメッセージとして受信して、その角度だけ体を回転します。また、"「関節名」＝「角度」"というメッセージを送信するとエージェントは関節を指定した角度に曲げます。

それではコンパイルします。

$ emacs Makefile

変更前

#オブジェクトファイルの指定
OBJS     = WatchController.so

変更後

#オブジェクトファイルの指定
OBJS     = SetCamera.so

$ make

↑

リビングルームの世界ファイル作成 †

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

$ cd ~/sigverse-<version>/bin
$ emacs xml/MultiCamera.xml

MultiCamera.xml

<?xml version="1.0" encoding="utf8"?>
 <world name="myworld6">
 
  <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="elnk1" value="HEAD_LINK"/>
 
    <set-attr-value name="epx1" value="0.0"/>
    <set-attr-value name="epy1" value="0.0"/>
    <set-attr-value name="epz1" value="10.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="elnk2" value="WAIST_LINK0"/>
 
    <set-attr-value name="epx2" value="0.0"/>
    <set-attr-value name="epy2" value="0.0"/>
    <set-attr-value name="epz2" value="-10.0"/>
 
    <set-attr-value name="evx2" value="0.0"/>
    <set-attr-value name="evy2" value="0.0"/>
    <set-attr-value name="evz2" value="-1.0"/>
 
    <set-attr-value name="elnk3" value="LARM_LINK7"/>
 
    <set-attr-value name="epx3" value="10.0"/>
    <set-attr-value name="epy3" value="0.0"/>
    <set-attr-value name="epz3" value="0.0"/>
 
    <set-attr-value name="evx3" value="1.0"/>
    <set-attr-value name="evy3" value="0.0"/>
    <set-attr-value name="evz3" value="0.0"/>
 
    <set-attr-value name="elnk4" value="RARM_LINK7"/>
 
    <set-attr-value name="epx4" value="-10.0"/>
    <set-attr-value name="epy4" value="0.0"/>
    <set-attr-value name="epz4" value="0.0"/>
 
    <set-attr-value name="evx4" value="-1.0"/>
    <set-attr-value name="evy4" value="0.0"/>
    <set-attr-value name="evz4" value="0.0"/>
 
    <set-attr-value name="language" value="c++"/>
    <set-attr-value name="implementation" value="./NewWorld/SetCamera.so"/>
  </instanciate>
 
  <instanciate class="Robot-nii.xml">
    <set-attr-value name="name" value="robot_1"/>
    <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="qw" value="0.0"/>
    <set-attr-value name="qx" value="0.0"/>
    <set-attr-value name="qy" value="1.0"/>
    <set-attr-value name="qz" value="0.0"/>
 
    <set-attr-value name="elnk1" value="HEAD_LINK"/>
 
    <set-attr-value name="epx1" value="0.0"/>
    <set-attr-value name="epy1" value="0.0"/>
    <set-attr-value name="epz1" value="10.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="elnk2" value="WAIST_LINK0"/>
 
    <set-attr-value name="epx2" value="0.0"/>
    <set-attr-value name="epy2" value="0.0"/>
    <set-attr-value name="epz2" value="-10.0"/>
 
    <set-attr-value name="evx2" value="0.0"/>
    <set-attr-value name="evy2" value="0.0"/>
    <set-attr-value name="evz2" value="-1.0"/>
 
    <set-attr-value name="elnk3" value="LARM_LINK7"/>
 
    <set-attr-value name="epx3" value="10.0"/>
    <set-attr-value name="epy3" value="0.0"/>
    <set-attr-value name="epz3" value="0.0"/>
 
    <set-attr-value name="evx3" value="1.0"/>
    <set-attr-value name="evy3" value="0.0"/>
    <set-attr-value name="evz3" value="0.0"/>
 
    <set-attr-value name="elnk4" value="RARM_LINK7"/>
 
    <set-attr-value name="epx4" value="-10.0"/>
    <set-attr-value name="epy4" value="0.0"/>
    <set-attr-value name="epz4" value="0.0"/>
 
    <set-attr-value name="evx4" value="-1.0"/>
    <set-attr-value name="evy4" value="0.0"/>
    <set-attr-value name="evz4" value="0.0"/>
 
    <set-attr-value name="language" value="c++"/>
    <set-attr-value name="implementation" value="./NewWorld/SetCamera.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"/>
 
  </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="vpx" value="0.0"/>
    <set-attr-value name="vpy" value="0.0"/>
    <set-attr-value name="vpz" value="0.0"/>
 
    
    <set-attr-value name="vpx" value="0.0"/>
    <set-attr-value name="vpy" value="0.0"/>
    <set-attr-value name="vpz" 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"/>
  </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"/>
  </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"/>
  </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"/>
  </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"/>
  </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"/>
  </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"/>
  </instanciate>
</world>

サブカメラを設置するリンクの名前を(elnk1, elnk2, elnk3, ... , elnk9)で設定します。
今回は、(elnk1, elnk2, elnk3, elnk4)=("HEAD_LINK", "WAIST_LINK0", "LARM_LINK7", "RARM_LINK7")として、elnk5～elnk9は使っていません。
指定できるリンクの一覧は、Joint定義一覧?を参照してください。 ( (epx1, epy1, epz1), (epx2, epy2, epz2), (epx3, epy3, epz3), ... (epx9, epy9, epz9) )で各カメラのリンクからの相対位置、
( (evx1, evy1, evz1), (evx2, evy2, evz2), (evx3, evy3, evz3), ... (evx9, evy9, evz9) )で各カメラの初期の向きを設定しています。

また、ヒューマノイド以外のエージェント（seDoll_Bear.xml)にもカメラを設置していますが、この場合はカメラを設置するリンク名を指定する必要はありません。カメラの位置、方向は(vpx,vpy,vpz), (vvx,vvy,vvz)で設定します。

↑

起動 †

それでは起動してみましょう。

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

SIGViewerでサーバに接続して、シミュレーションを開始します。リビングルームに向かい合って立っている２体のヒューマノイドロボット型のエージェントが見えると思います。

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

ロボットをクリックするとロボットにつけられたカメラから映し出された画像が左側のサブカメラに表示されます。

"start"ボタンを押してシミュレーションを開始して、SIGViewerからエージェントに"rotation"や"move"などのメッセージを送信してエージェントが向きを変えたり、移動したりすることを確認してみてください。ロボットを動かすと左の画像でサブカメラの視線がつられて動いているのがわかると思います。

↑

サブカメラの切り替え †

"camera"タブから左の４つのビューポートに映すカメラを切り替えることが出来ます。まず、カメラを表示したいエージェントをクリックします。すると"Target Object:"のところにクリックしたエージェントの名前が表示されます。

"SubCam ID:"は上からビューポート１、ビューポート２、ビューポート３、ビューポート４に対応しており、それぞれ好きなカメラ番号で映し出される画面を表示することができます。
カメラ番号を変更するとビューポートが更新されます。
また"NoCam"に切り替えた場合はビューポートが見えなくなります。

↑

メインビューの切り替え †

Viewerにメインで表示されている画面をエージェントに取り付けられたカメラからの画面に切り替えることができます。メインビューで表示したいエージェントをクリックし、cameraタブの"MainCam ID:"をカメラ番号（ヒューマノイド以外の場合はAgentView)を指定するとメインビューがエージェントのカメラ視点に切り替わります。

メインビューを"Robot_1"のカメラ１の画像に切り替えました。 WorldViewに戻すと元の画面に戻ります。

ここではロボットに複数のカメラを設置しましたが人間型エージェントでも同じことが行えます。