![[PukiWiki]](image/Concept_logo.jpg "[PukiWiki]"){kind=logo}
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="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="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"/> </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) )で各カメラの初期の向きを設定しています。
それでは起動してみましょう。
$ ./sigserver.sh -w MultiCamera.xml -p 9001
SIGViewerでサーバに接続して、シミュレーションを開始します。リビングルームに向かい合って立っている２体のヒューマノイドロボット型のエージェントが見えると思います。
SIGViewerで接続してみるとリビングルームにロボットが立っているのが見えます。
ロボットをクリックするとロボットにつけられたカメラから映し出された画像が左側のサブカメラに表示されます。
"start"ボタンを押してシミュレーションを開始して、SIGViewerからエージェントに"rotation"や"move"などのメッセージを送信してエージェントが向きを変えたり、移動したりすることを確認してみてください。ロボットを動かすと左の画像でサブカメラの視線がつられて動いているのがわかると思います。
"camera"タブから左の４つのビューポートに映すサブカメラを切り替えることが出来ます。
上からビューポート１、ビューポート２、ビューポート３、ビューポート４に対応しています。
サブカメラを持つエージェントとサブカメラ番号を指定するとビューポートが更新されます。
無効なサブカメラに切り替えた場合はビューポートが見えなくなります。
Up:Tutorial? Previous：視覚に関する操作 Next：エージェント視点の画像取得
Site admin: inamura
PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.