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

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()は同様に視野全体に渡る，二次元配列の距離データを取得することができます。

※このサンプルはサービスプロバイダ機能を使用するため、ポートフォワーディングでサーバに接続している場合は双方向のポートフォワーディングが必要です。設定方法は[[こちら>ポートフォワーディングでサーバに接続する方法]]を参照ください。

**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);
 private: 
 
   //移動速度
   double vel;
 }; 
 
 void RobotController::onInit(InitEvent &evt)
 {
   vel      = 10.0;
   srand(time(NULL));
 }  
 
 //定期的に呼び出される関数
 double RobotController::onAction(ActionEvent &evt)
 { 
 
   try { 
 
     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;
     double dz = 0;
 
 
     //移動する方向を決定します。
     dx = sin(theta) * vel;
     dz = cos(theta) * vel;
 
     //移動します。
     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 ()
 {
   return new RobotController;
 }
このコントローラは以下の行

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

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

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

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


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

 $ cd ..
 $ emacs xml/distanceSensor.xml

distanceSensor.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="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="-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.0"/>
    <set-attr-value name="evy1" value="-1.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.0"/>
    <set-attr-value name="evy2" value="0.0"/>
    <set-attr-value name="evy2" value="-1.0"/>
    <set-attr-value name="evz2" value="1.0"/>
 
    <set-attr-value name="language" value="c++"/>
    <set-attr-value name="implementation" value="./distanceSensor.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="seSofa_2seater.xml">
       <set-attr-value name="name" value="sofa_0"/>
       <set-attr-value name="dynamics" value="false"/>
   <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="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="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="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>

***実行 [#l3f6882d]
それでは実行してみましょう。
 $ ./sigserver.sh -p 9001 -w xml/distanceSensor.xml

#ref(distance_1.jpg)

シミュレーションを実行してみるとロボットが進んで、オブジェクトに近づくとロボットが方向転換しているのがわかると思います。

**距離範囲、カメラIDの指定 [#baeff175]
デフォルトでは0～255(SIGVerseの距離）までの距離データしか取得することができません。
さらに遠くの距離を取得したい場合などはdistanceSensor()の引数に取得したい距離データの範囲を指定することができます。

distanceSensor()を例えば以下のように修正します。
     unsigned char distance = distanceSensor();
　　　　↓
     unsigned char distance = distanceSensor(50.0, 500.0, 2);

最初２つの引数で距離センサが取得できる距離の範囲を指定します。この例ではカメラから距離が50.0～500.0までの距離データを取得することができます。このとき取得できる距離データの値は先ほどと同様0～255です。つまりカメラからの距離が50.0以下の場合は戻り値が0になり、500.0以上の場合は255となります。範囲を大きくすると広範囲の距離データを取得することができますが分解能が悪くなります。

さらにカメラが複数ある場合などは3番目の引数でカメラIDを指定します。

この後で出てくるdistanceSensor1D, distanceSensor2Dも同様の設定が可能です。

**1次元距離センサ [#cb2b5721]
エージェントの視線の水平面に沿って距離を取得するサンプルです。水平面の距離データが配列データで一度に取得することができます。
***コントローラ修正 [#s7488bb0]
コントローラを修正します。

 $ cd NewWorld
 $ emacs -nw distanceSensor.cpp
以下の部分を修正
    //視線方向のオブジェクトまでの距離を取得します。
    unsigned char = distanceSensor();
　　　　↓
    //水平面の距離データを取得します。
    DepthImage *depth = distanceSensor1D();
 
    unsigned char distance = 255;
 
    //水平の距離データの最小値を求めます。
    for(int i = 0; i < 320; i++){
      //ピクセルの位置を指定して距離データを取得します。
      unsigned char tmp_dis = depth->getDepthFromPixel(i);
      if(distance > tmp_dis) {
        distance = tmp_dis;
      }
    }

distanceSensor1D()で1次元距離データを取得します。戻り値はクラスDepthImageです。
デフォルトではカメラの視野角内の幅320ピクセルのデータが取得できます。(2011年9月の時点では320のみ対応)。距離データを取得するにはgetDepthFromPixelを使って距離データを取得します。

pixelではなくカメラからの方向を指定して距離データを取得する場合はgetDepthFromAngle()を使います。このとき引数で方向を指定します。単位はdegree、 視線方向が0、左が＋。（2次元距離データの場合は2番目の引数に高さ方向の角度指定)

このサンプルは水平面の距離データの最小値（いちばん近い距離）が120以下の場合に方向を変えるサンプルです。

コンパイルします。
 $ make


***実行 [#o60b67bd]
実行してみましょう。
 $ cd ../
 $ ./sigserver.sh -p 9001 -w xml/distanceSensor.xml

先ほどのサンプルでは近くにオブジェクトがあっても視線方向と外れていたら向きを変えませんでしたが、
こちらのサンプルではロボットはより衝突を回避できているのがわかります。

**2次元距離センサ [#z93a6d1d]
視野全体の距離データを取得するサンプルを紹介します。
Backup diff of 距離センサ(v120330, v1.4.8) (No. 4)
