距離センサ(v120330, v1.4.8)

Up:Tutorial?　　　Previous：車輪移動ロボット　　　 Next: 眼球運動　　　

距離センサ

距離センサ †

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

※このサンプルはSIGVerseのバージョン110927, SIGViewer-1.4.1以降で正常に動作します。

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

↑

視線方向の距離センサ †

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型です。つまり取得できる距離データは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="-1.0"/>
    <set-attr-value name="evz2" value="1.0"/>
 
    <set-attr-value name="language" value="c++"/>
    <set-attr-value name="implementation" value="./NewWorld/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

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

↑

距離範囲、カメラIDの指定 †

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

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

　　　　↓

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

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

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

↑

1次元距離センサ †

エージェントの視線の水平面に沿って距離を取得するサンプルです。水平面の距離データが配列データで一度に取得することができます。

↑

コントローラ修正 †

コントローラを修正します。

$ cd NewWorld
$ emacs -nw distanceSensor.cpp

distanceSensor.cpp

　　　　↓

distanceSensor1D()で視線方向の水平面に沿った1次元の深度マップが得られます。戻り値はクラスDepthImageです。デフォルトではカメラの視野角内の幅320ピクセル（高さ1ピクセル）のデータが取得できます。(2011年9月の時点では320のみ対応)。距離データを取得するにはgetDistanceFromPixel()を使います。引数はピクセル位置を指定します。0が横方向の視野角内のいちばん左、320がいちばん右のピクセルになります。

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

コンパイルします。

$ make

↑

実行 †

実行してみましょう。

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

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

↑

2次元距離センサ †

視野全体の距離データを取得するサンプルを紹介します。

↑

コントローラ †

新しくコントローラを作成します。

$ cd NewWorld
$ emacs distanceSensor2D.cpp

distanceSensor2D.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 )
 
 class RobotController : public Controller
 {
 public:
   void onInit(InitEvent &evt);
   double onAction(ActionEvent &evt);
   void onRecvMessage(RecvMessageEvent &evt);
 
 private:
 
   //移動速度
   double vel;
 
   //体を回転させるときの回転角
   int dy;
 };
 
 void RobotController::onInit(InitEvent &evt)
 {
   vel = 10.0;
   dy = 45;
 }
 
 //定期的に呼び出される関数
 double RobotController::onAction(ActionEvent &evt)
 {
   return 1.0; 
 }
 
 //メッセージ受信時に呼び出される関数
 void RobotController::onRecvMessage(RecvMessageEvent &evt)
 {
   int n = evt.getSize();
   static int iImage = 0;
   if (n>0)
     {
 
       //取得したメッセージを表示します。
       std::string msg = evt.getString(0);
       LOG_MSG(("msg : %s", msg.c_str())); 
 
 
       //メッセージ"capture"を受信するとdistanceSensorを行います。
       if (strcmp(msg.c_str(), "capture") == 0)
         {
          SimObj *my = getObj(myname());
 
          //エージェント視点の深度画像を取得します。
          DepthImage *img = distanceSensor2D();
          double distance = 255.0;
          int min_i = 0;
          int min_j = 0;
 
          //取得した深度画像の高さと幅取得
          int height = img->getHeight();
          int width = img->getWidth();
 
          //視野角取得(高さ方向）
          double fov = my->FOV();
 
          //焦点距離の計算（pixel単位）
          double fl = (img->getHeight()/2)/tan(DEG2RAD(fov/2));
 
          if (img)
            {
              for(int i = 0; i < height; i++){
                for(int j = 0; j < width; j++){
 
                    //ピクセル位置を指定して距離データ取得
                    double tmp_dis = img->getDistanceFromPixel(j,i);
                  if(distance > tmp_dis){
                    distance = tmp_dis;
                    //最小距離の時のpixel位置を取得します。
                    min_i = i;
                    min_j = j;
                  }
                }
              }
              //最も近い物体までの距離を表示
              LOG_MSG(("distance = %.1f", distance));
 
              //最も近いオブジェクトの方向を計算し手を伸ばします。
              my->setJointAngle("RARM_JOINT2", atan(((height/2)-min_j)/fl));
              my->setJointAngle("RARM_JOINT0", DEG2RAD(-90.0) - atan(((width/2)-min_i)/fl));
 
               //Windows BMP 形式で保存します。
               char fname[256];
               sprintf(fname, "view%03d.bmp", iImage++);
               img->saveAsWindowsBMP(fname);
 
               //必要なくなったら削除します。
               delete img;
             }
         }
 
       //メッセージ"rotation"を受信すると回転します。
       if (strcmp(msg.c_str(), "rotation") == 0){
 
         SimObj *my = getObj(myname());
 
         // 自分の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){
 
         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.0;
         double dz = 0.0;
 
         //移動する方向を決定します。
         dx = sin(theta) * vel;
         dz = cos(theta) * vel;
 
         //移動します。
         my->setPosition( x + dx, y , z + dz );
       }
     }
 } 
  
 extern "C" Controller * createController ()
 {
   return new RobotController;
 }

このサンプルはcaptureViewのときと同様にエージェントに"move","rotation"等のメッセージを送信してロボットを操作します。また、"capture"を送信した場合はdistanceSensor2D()を使って視野全体の距離データ（深度画像）を取得し、その中で最も近いオブジェクトがある方向を計算し、その方向をロボットが指さします。画像中に同じ距離が複数ある場合は左上を優先して指さします。デフォルトで取得できる距離データのサイズは320×240(2011年9月の時点では320×240のみ対応)です。

また、captureViewで取得した画像と同様に　

で距離データを保存し、可視化することができます。

それではMakefileを修正してコンパイルします。

$ make

↑

世界ファイルの編集 †

次に世界ファイルを修正します。

$ cd ..
$ emacs xml/distanceSensor.xml

コントローラ指定箇所を修正します。

distanceSensor.xml

　　　　↓

視線ベクトルを修正します。（※下を向いていると地面を検出してしまうので。。）

　　　　↓

↑

実行 †

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

$ cd ~/sigverse/bin
$ ./sigserver.sh -p 9001 -w xml/distanceSensor.xml

ロボットがリビングに立っているので好きなところでメッセージ"capture"を送信して見てください。ロボットが最も近いオブジェクトの方向を指します。

次に保存した深度画像を見てみましょう。以下のような濃淡画像が見えると思います。

　　　　　　　　　　　　　　　　深度画像の例

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

Up:Tutorial?　　　Previous：車輪移動ロボット　　　 Next: 眼球運動　　　

recent(20)