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

List of Backups
View the source.
距離センサ(v120330, v1.4.8) has been deleted.

Up:Tutorial?　　　Previous：車両のダイナミクス（ダンベルモデル）　　

距離センサ
- 0次元距離センサ

距離センサ †

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

↑

0次元距離センサ †

distanceSensor()を使ってカメラの視線方向のオブジェクトまでの距離を取得するサンプルを紹介します。

↑

コントローラ作成 †

まずはコントローラを作成します。

$ 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;
}

このコントローラは以下の行

unsigned char distance = distanceSensor();

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

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

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

↑

世界ファイル †

次に世界ファイルを作成します。

$ 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="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="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="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>

↑

実行 †

それでは実行してみましょう。

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

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

距離センサ †

0次元距離センサ †

コントローラ作成 †

世界ファイル †

実行 †