Up:[[Tutorial]]　　 　Previous：[[車輪移動ロボット]]　　　 Next: [[眼球運動]]　　　
#contents

*距離センサ [#r0d5dd55]


SIGVerseでは距離センサはカメラを用います。
ここではSIGViewerサービスから距離データを取得するサンプルを紹介します。


カメラから取得可能な距離データは3つあります。一つ目はカメラから視線方向にあるオブジェクトまでの点と点の距離(スカラ値)、2つ目はエージェントの視線から水平面に沿って，物体までの距離を連続的に得るベクトル値（一次元配列）、3つ目は視野全体に渡る，二次元配列の距離データです。


※このサンプルはサービスプロバイダ機能を使用するため、SSH接続の場合は追加でポートフォワーディングの設定が必要です。

**視線方向の距離センサ [#r07b5fe9]
distanceSensor()を使ってカメラの視線方向にあるオブジェクトまでの距離を取得するサンプルを紹介します。
***コントローラ作成 [#j5a63794]
まずはコントローラを作成します。
 $ cd sigverse-<version>/bin/NewWorld
 $ emacs distanceSensor.cpp

distanceSensor.cpp

#highlight(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;    
  ViewService *m_view;  
};     
    
void RobotController::onInit(InitEvent &evt)    
{    
  m_view = (ViewService*)connectToService("SIGViewer", 9005);  
  vel      = 1.0;    
  srand(time(NULL));    
}      
    
//定期的に呼び出される関数    
double RobotController::onAction(ActionEvent &evt)    
{     
  SimObj *my = getObj(myname());    
    
  //自分の位置を得ます
  Vector3d pos;
  my->getPosition(pos);    
    
  //y軸周りの自分の回転を得ます（クオータニオン）    
  double qy = my->qy();    
  double qw = my->qw();    
  double tmp = qy*qw;
  if(tmp < 0) qy = -qy;
  
  //クオータニオンから回転角を導出します    
  double theta = 2*asin(qy);    

  double dx = 0;    
  double dz = 0;    
    
  //移動する方向を決定します  
  dx = sin(theta) * vel;    
  dz = cos(theta) * vel;    
    
  //移動します    
  my->setPosition( pos.x() + dx, pos.y() , pos.z() + dz );    
    
  unsigned char distance = 255;  
  if(m_view != NULL) {  
    //視線方向のオブジェクトまでの距離を取得します    
    distance = m_view->distanceSensor();    
    LOG_MSG(("distance = %d",distance));    
  }    
  //距離が80以下であれば向きを変えます    
  if(distance < 80){    
      my->setAxisAndAngle(0.0, 1.0, 0.0, (double)rand()/RAND_MAX * 2*PI, true);    
    }     
  return 0.1;    
}     
    
extern "C" Controller * createController ()    
{    
  return new RobotController;    
}    
}}

このサンプルでは62行目のdistanceSensor()で距離データを取得しています。
#highlight(cpp:firstline[62]){{
      distance = m_view->distanceSensor();
}}
戻り値はunsigned char型です。つまり取得できる距離データは0～255までの整数です。デフォルトでは255cmまでの距離を取得することができ、255よりも遠くにオブジェクトがある場合はすべて255が返ってきます。
引数を変えることによってこれらの距離を変更することができます。

より遠くの距離を取得したい場合はdistanceSensor()の引数に取得したい距離データの範囲を指定することができます。

distanceSensor()を例えば以下のように修正します。


#highlight(cpp:firstline[62]){{
      distance = m_view->distanceSensor();
}}
　　　　↓
#highlight(cpp:firstline[62]){{
     distance = m_view->distanceSensor(50.0, 500.0, 2);
}}
最初２つの引数で距離センサが取得できる距離の範囲を指定します。この例ではカメラから距離が50.0～500.0(cm)までの距離データを取得できることになります。このとき戻り値は先ほどと同様0～255です。つまりカメラからの距離が50.0cm以下の場合は戻り値が0になり、500.0cm以上の場合は255となります。取得できる距離データの値と実際の距離は常に比例関係にあります。範囲を大きくすると広範囲の距離データを取得することができますが分解能が悪くなります。
最初２つの引数で距離センサが取得できる距離の範囲を変更することができます。第一引数にoffset値、第2引数に取得する距離データの範囲を指定します。この例ではカメラから距離が50.0～550.0(cm)までの距離データを取得できることになります。このとき戻り値は先ほどと同様0～255です。つまりカメラからの距離が50.0cm以下の場合は戻り値が0になり、550.0cm以上の場合は255となります。取得できる距離データの値と実際の距離は常に比例関係にあります。範囲を大きくすると広範囲の距離データを取得することができますが分解能が悪くなります。

最後の引数はカメラＩＤ番号です。
何も指定しない場合はカメラのＩＤ番号１から距離データを取得します。

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

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


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

 $ cd ..
 $ emacs xml/distanceSensor.xml

distanceSensor.xml

#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
<world name="myworld5">

  <gravity x="0.0" y="-980.7" z="0.0"/>
  <instanciate class="WheelRobot-nii-v1.xml">

    <!--エージェント名-->
    <set-attr-value name="name" value="robot_000"/>

    <!--C++言語の指定-->
    <set-attr-value name="language" value="c++"/>

    <!--コントローラの指定-->
    <set-attr-value name="implementation"
                    value="./NewWorld/distanceSensor.so"/>

    <!--動力学演算をfalseに設定-->
    <set-attr-value name="dynamics" value="false"/>
    
    <!--エージェントの位置(x,y,z)-->
    <set-attr-value name="x" value="0.0"/>
    <set-attr-value name="y" value="30.0"/>
    <set-attr-value name="z" value="-40.0"/>
    
    <set-attr-value name="qw" value="0.0"/>
    <set-attr-value name="qy" value="1.0"/>
    
    <!--カメラのID番号,リンク名、方向、位置, 視野角(y方向)の設定, 縦横比-->
    <camera id="1"
            link="HEAD_LINK"
            direction="0 0 1"
            position="0.0 0.0 5.0"
            fov="80"
            aspectRatio="1.5"/>
  </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="62.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="seBookShelf_A.xml">
    <set-attr-value name="name" value="BookShelf_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-200.0"/>
    <set-attr-value name="y" value="80.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="seBookShelf_B.xml">
    <set-attr-value name="name" value="BookShelf_1"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-0.0"/>
    <set-attr-value name="y" value="32.0"/>
    <set-attr-value name="z" value="100.0"/>
    <set-attr-value name="qw" value="0.0"/>
    <set-attr-value name="qy" value="1.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="8.0"/>
    <set-attr-value name="z" value="-250.0"/>
  </instanciate>

  <instanciate class="seTana_c02.xml">
    <set-attr-value name="name" value="tana_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="150.0"/>
    <set-attr-value name="y" value="72.0"/>
    <set-attr-value name="z" value="-100.0"/>
    <set-attr-value name="qw" value="0.707"/>
    <set-attr-value name="qy" value="-0.707"/>
  </instanciate>
</world>
}}

今回は車輪付き移動ロボットを使用します。


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

#ref(./distance_1.JPG,60%)

シミュレーションを実行するとロボットが前に進み始めます。そしてオブジェクトに衝突しそうになると方向転換します。



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

 $ cd NewWorld
 $ emacs -nw distanceSensor.cpp

distanceSensor.cpp

#highlight(cpp:firstline[61]){{
    //視線方向のオブジェクトまでの距離を取得します    
    distance = m_view->distanceSensor();    
    LOG_MSG(("distance = %d",distance));
}}
　　　　↓
#highlight(cpp:firstline[61]){{
    //視線方向水平面の距離データを取得します
    ViewImage *img = m_view->distanceSensor1D();
    char *buf = img->getBuffer();

    //データの長さを取得します 
    int length = img->getBufferLength();

    //水平面内の距離データの最小値を求めます
    for(int i = 0; i < length; i++){
      unsigned char tmp_distance = (unsigned char)buf[i];
      if(tmp_distance < distance){
        distance = buf[i];
      }
    }
    LOG_MSG(("distance = %d",distance));
    delete img;
}}
distanceSensor1D()で視線方向の水平面に沿った1次元の距離データを取得します。戻り値はViewImageクラスです。
デフォルトではカメラの視野角内の幅320ピクセルのデータが取得します。距離データを取得するにはgetBuffer()を使います。0が横方向の視野角内のいちばん左、320がいちばん右のピクセルになります。

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

コンパイルします。
 $ make


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

先ほどのサンプルでは近くにオブジェクトがあっても視線方向と外れていたら向きを変えませんでした。
このサンプルでは近くにオブジェクトがあれば向きを変えます。
4つのオブジェクトに囲まれたロボットはなかなか外に出られなくなります。

**2次元距離センサ [#z93a6d1d]
次は視野全体の距離データを取得します。
***コントローラ [#n495b830]

distanceSensor.cppの以下を修正します。
#highlight(cpp:firstline[61]){{
    //視線方向水平面の距離データを取得します
    ViewImage *img = m_view->distanceSensor1D();
}}
　        ↓
#highlight(cpp:firstline[61]){{
    //視野全体の距離データを取得します
    ViewImage *img = m_view->distanceSensor2D();
}}



captureViewで取得した画像と同様に距離データをファイルに保存したい場合は以下の行を適当に追加します。
#highlight(cpp:firstline[81]){{
 img->saveAsWindowsBMP("distance.bmp");
}}


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


保存した距離データ画像を見てみると以下のような距離データが見ることができます。
#ref(distance.bmp)


#highlight(end)

遠いオブジェクトほど色が薄くなり、近いオブジェクトほど色が濃くなっています。


*Old Version [#gadb4153]
-[[距離センサ(v120330, v1.4.8)]]

Up:[[Tutorial]]　　 　Previous：[[車輪移動ロボット]]　　　 Next: [[眼球運動]]　　　
Front page   Edit Diff Backup Upload Copy Rename Reload   New List of pages Search Recent changes   Help   RSS of recent changes