Backup diff of 距離センサ(v120330, v1.4.8)(No. 3)

List of Backups
View the source.
View the backup.
距離センサ(v120330, v1.4.8) has been deleted.
The added line is THIS COLOR.
The deleted line is THIS COLOR.
Up:[[Tutorial]]　　 　Previous：[[車両のダイナミクス（ダンベルモデル）]]　　
#contents

*距離センサ [#r0d5dd55]
ここでは、距離センサ機能を使ったサンプルを紹介します。エージェントに設置されたカメラの位置からそのカメラの視線方向におけるオブジェクトまでの距離を取得することができます。
ここでは距離センサ機能を使ったサンプルを紹介します。距離センサAPIはdistanceSensor(), distanceSensor1D, distanceSensor2D()の3つがあります。distanceSensor()はカメラから視線方向の点と点の距離としてのスカラ値、distanceSensor1Dはエージェントの視線から水平面に沿って，物体までの距離を連続的に得るベクトル値（一次元配列）、distanceSensor2D()は同様に視野全体に渡る，二次元配列の距離データを取得することができます。

**コントローラ作成 [#r07b5fe9]

まずはコントローラを作成します。([[captureView>エージェント視点の画像取得]]のコントローラとほとんど同じです。）
**0次元距離センサ [#r07b5fe9]
distanceSensor()を使ってカメラの視線方向のオブジェクトまでの距離を取得するサンプルを紹介します。
***コントローラ作成 [#j5a63794]
まずはコントローラを作成します。
 $ cd sigverse-<version>/bin/NewWorld
 $ emacs distanceSensor.cpp

distanceSensor.cpp

 #include <Controller.h>
 #include <ControllerEvent.h>
 #include <Logger.h>
 #include <ViewImage.h>
 #include <math.h>
 #include <stdio.h>
 #include <stdlib.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:  
 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;
 } 
   srand(time(NULL));
 }  
 
 //定期的に呼び出される関数
 double RobotController::onAction(ActionEvent &evt)
 { 
 
   SimObj *my = getObj(myname());
   try { 
 
   //視線ベクトルのx成分を取得します。
   double vvx = my->evx1(); 
     SimObj *my = getObj(myname());
 
   double dvvx_tmp;
   //視線が右に移動中の場合
   if (move_eye == false)
     {
       dvvx_tmp = -1 * dvvx;
     }
   //左に移動中の場合
   else dvvx_tmp = dvvx;
     //自分の位置を得ます。
     double x = my->x();
     double y = my->y();
     double z = my->z();
 
   //視線を変化させます。
   vvx = vvx + dvvx_tmp;
   my->evx1(vvx); 
     //y軸周りの自分の回転を得ます。（クオータニオン）
     double qy = my->qy();
 
     //クオータニオンから回転角を導出します。
     double theta = 2*asin(qy);
 
   //視線ベクトルのx成分を-1.0から1.0まで移動させます。
   if(vvx >= 1.0)
     {
       move_eye = false;
     }
   if( vvx <= -1.0)
     {
       move_eye = true;
     } 
     double dx = 0;
     double dz = 0;
 
   return 0.5; 
 
 }
     //移動する方向を決定します。
     dx = sin(theta) * vel;
     dz = cos(theta) * vel;
 
 //メッセージ受信時に呼び出される関数
 void RobotController::onRecvMessage(RecvMessageEvent &evt)
     //移動します。
     my->setPosition( x + dx, y , z + dz );
 
     //距離を取得します。
     unsigned char distance = distanceSensor();
     LOG_MSG(("distance = %d",distance));
 
     //距離が120以下であれば向きを変えます
     if(distance < 120)
       {
         my->setAxisAndAngle(0.0, 1.0, 0.0, (double)rand()/RAND_MAX * PI - PI/2);
       } 
 
   } catch (SimObj::Exception &) {
   }
  
   return 1.0;
 
 } 
 
 extern "C" Controller * createController ()
 {
   int n = evt.getSize();
   static int iImage = 0;
   if (n>0)
     {  
   return new RobotController;
 }
このコントローラは以下の行

&color(red){unsigned char distance = distanceSensor();};

で視線方向のオブジェクトまでの距離を取得しています。戻り値はunsigned char型です。つまり取得できる距離データは0～255までの整数です。デフォルトでは255（SIGVerseの距離の単位）までの距離を取得することができ、255よりも遠いオブジェクトはすべて255と表示されます。

このコントローラではロボットは向いている方向に進み、視線方向のオブジェクトまでの距離が120よりも近くなるとロボット向きをランダムに変えます。

Makefileに作成したファイルを追加してコンパイルします。（手順省略）


***世界ファイル [#md4f157f]
次に世界ファイルを作成します。

 $ cd ..
 $ emacs xml/distanceSensor.xml

distanceSensor.xml

 <?xml version="1.0" encoding="utf8"?>
 <world name="VisTest2">
 
       //取得したメッセージを表示します。
       std::string msg = evt.getString(0);
       LOG_MSG(("msg : %s", msg.c_str()));
  <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"/>
 
       //メッセージ"distance"を受信すると距離センサで距離を測定します。
       if (strcmp(msg.c_str(), "distance") == 0)
         {
           //距離を取得します。
           unsigned char dis = distanceSensor();
           LOG_MSG(("distance = %d", dis));
         } 
    <set-attr-value name="x" value="0.0"/>
    <set-attr-value name="y" value="60.0"/>
    <set-attr-value name="z" value="-40.0"/>
 
       //メッセージ"rotation"を受信すると回転します。
       if (strcmp(msg.c_str(), "rotation") == 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"/>
 
         SimObj *my = getObj(myname());
    <set-attr-value name="elnk1" value="HEAD_LINK"/>
 
         // 自分のy軸周りの回転を得ます(クオータニオン)
         double qy = my->qy();
    <set-attr-value name="epx1" value="-5.0"/>
    <set-attr-value name="epy1" value="0.0"/>
    <set-attr-value name="epz1" value="5.0"/>
 
         //くオータニオンから回転角(ラジアン)を導出します
         double theta = 2*asin(qy);
    <set-attr-value name="evx1" value="0.0"/>
    <set-attr-value name="evy1" value="-1.0"/>
    <set-attr-value name="evz1" value="1.0"/>
 
         //体全体を回転させます。
         double y = theta + DEG2RAD(45);
         if( y >= PI)
          {
            y = y - 2 * PI;
          }
         my->setAxisAndAngle(0, 1.0, 0, y);
    <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"/>
 
       //メッセージ"move"を受信すると自分の向いている方向に進みます。
       if (strcmp(msg.c_str(), "move") == 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"/>
 
         SimObj *my = getObj(myname());
    <set-attr-value name="language" value="c++"/>
    <set-attr-value name="implementation" value="./distanceSensor.so"/>
 </instanciate>
 
         //自分の位置を得ます。
         double x = my->x();
         double y = my->y();
         double z = my->z();
 <instanciate class="seTV.xml">
   <set-attr-value name="name" value="TV_0"/>
   <set-attr-value name="dynamics" value="false"/>
 
          //y軸周りの自分の回転を得ます。（クオータニオン）
   <set-attr-value name="x" value="-20.0"/>
   <set-attr-value name="y" value="82.0"/>
   <set-attr-value name="z" value="-250.0"/>
 
         double qy = my->qy();
   <set-attr-value name="visStateAttrName" value="switch"/>
   <set-attr-value name="switch" value="on"/>
 </instanciate>
 
         //クオータニオンから回転角を導出します。
         double theta = 2*asin(qy);
 <instanciate class="seSofa_2seater.xml">
       <set-attr-value name="name" value="sofa_0"/>
       <set-attr-value name="dynamics" value="false"/>
 
         //移動距離を初期化します。
         double dx = 0.0;
         double dz = 0.0;
  
         //移動する方向を決定します。
         dx = sin(theta) * vel;
         dz = cos(theta) * vel;
       <set-attr-value name="x" value="-200.0"/>
       <set-attr-value name="y" value="20.0"/>
       <set-attr-value name="z" value="-100.0"/>
 
         //移動します。
         my->setPosition( x + dx, y , z + dz );
       <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>
 
 extern "C" Controller * createController ()
 {
   return new RobotController;
 }
 <instanciate class="seMagazine_rack_B.xml">
   <set-attr-value name="name" value="rack_0"/>
   <set-attr-value name="dynamics" value="false"/>
 
   <set-attr-value name="x" value="150.0"/>
   <set-attr-value name="y" value="75.0"/>
   <set-attr-value name="z" value="-250.0"/>
 </instanciate>
 
 <instanciate class="sePlant_B.xml">
   <set-attr-value name="name" value="plant_0"/>
   <set-attr-value name="dynamics" value="false"/>
 
   <set-attr-value name="x" value="150.0"/>
   <set-attr-value name="y" value="30.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="seTrashbox_c01.xml">
   <set-attr-value name="name" value="trash_0"/>
   <set-attr-value name="dynamics" value="false"/>
 
   <set-attr-value name="x" value="50.0"/>
   <set-attr-value name="y" value="30.0"/>
   <set-attr-value name="z" value="70.0"/>
 
   <set-attr-value name="scalex" value="0.70"/>
   <set-attr-value name="scaley" value="0.70"/>
   <set-attr-value name="scalez" value="0.70"/>
 
   <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="seTrashbox_c02.xml">
   <set-attr-value name="name" value="trash_1"/>
   <set-attr-value name="dynamics" value="false"/>
 
   <set-attr-value name="x" value="0.0"/>
   <set-attr-value name="y" value="30.0"/>
   <set-attr-value name="z" value="70.0"/>
 
   <set-attr-value name="scalex" value="0.70"/>
   <set-attr-value name="scaley" value="0.70"/>
   <set-attr-value name="scalez" value="0.70"/>
 
   <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="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>
 
 </world>

このエージェントは視線ベクトルを常に左右に動かし、"rotation"や"move"といったメッセージを送信して操作します。
操作方法は[[captureView>エージェント視点の画像取得]]を参考にしてください。このエージェントにメッセージ"distance"を送信するとdistanceSensor()を使ってカメラから視線方向へのオブジェクトの位置を測定します。
***実行 [#l3f6882d]
それでは実行してみましょう。
 $ ./sigserver.sh -p 9001 -w xml/distanceSensor.xml
Backup diff of 距離センサ(v120330, v1.4.8) (No. 3)
