Backup source of Distance sensor (No. 2)

[ Front page ]   [ New | List of pages | Search | Recent changes | Help ]

• List of Backups
• View the diff.
• View the diff current.
• View the backup.
• Go to Distance sensor.
  • 1 (2013-02-25 (Mon) 16:28:19)
  • 2 (2013-02-25 (Mon) 18:45:14)
  • 3 (2013-02-26 (Tue) 14:38:06)
  • 4 (2013-03-01 (Fri) 15:45:12)
  • 5 (2013-09-25 (Wed) 10:27:48)
  • 6 (2013-09-25 (Wed) 16:34:55)
  • 7 (2013-12-04 (Wed) 01:54:10)
  • 8 (2013-12-13 (Fri) 18:21:59)
  • 9 (2014-02-06 (Thu) 13:27:14)

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

*距離センサ [#r0d5dd55]

※このチュートリアルはv2.1.0以降で動作可能です。

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


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


**視線方向の距離センサ [#r07b5fe9]
distanceSensor()を使ってカメラの視線方向にあるオブジェクトまでの距離を取得するサンプルを紹介します。
***コントローラ作成 [#j5a63794]
作業ディレクトリに移動しコントローラを作成します。
 $ cd ~/MyWorld
 $ 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");  
  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));    
  }    
  //距離が100以下であれば向きを変えます    
  if(distance < 100){    
      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);
}}
最初２つの引数で距離センサが取得できる距離の範囲を変更することができます。第一引数にoffset値、第2引数に取得する距離データの範囲を指定します。この例ではカメラから距離が50.0～550.0(cm)までの距離データを取得できることになります。このとき戻り値は先ほどと同様0～255です。つまりカメラからの距離が50.0cm以下の場合は戻り値が0になり、550.0cm以上の場合は255となります。取得できる距離データの値と実際の距離は常に比例関係にあります。範囲を大きくすると広範囲の距離データを取得することができますが分解能が悪くなります。

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

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

コンパイルします。

 $ ./sigmake.sh distanceSensor.cpp

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

 $ emacs 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="./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="45"
            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="87.3"/>
    <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="75.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="59.5"/>
    <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="25.1"/>
    <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="81.5"/>
    <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 -w ./distanceSensor.xml

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

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



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

 $ emacs distanceSensor1D.cpp

distanceSensor1D.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");  
  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 );    
  
  // カメラの垂直方向(radian)の視野角を取得します
  double fovy = my->getCamFOV() * PI / 180.0;

  // アスペクト比を取得します
  double ar = my->getCamAS();
  
  // カメラの水平方向の視野角(degree)を計算します
  double fovx = 2 * atan(tan(fovy*0.5)*ar) * 180.0 / PI;

  unsigned char distance = 255;  
  if(m_view != NULL) {  

    // 視線方向水平面の距離データを取得します  
    ViewImage *img = m_view->distanceSensor1D();  
    char *buf = img->getBuffer();  
  
    // データの長さを取得します   
    int length = img->getBufferLength();  

    // 視線方向からの角度
    double theta = 0.0;
  
    // 水平面内の距離データの最小値を求めます  
    for(int i = 0; i < length; i++){  
      unsigned char tmp_distance = (unsigned char)buf[i];  
      if(tmp_distance < distance){  
	// 配列の位置から視線方向からの角度を取得します
	theta = fovx*i/319.0 - fovx/2.0;
	distance = buf[i];  
      }  
    }  
    LOG_MSG(("theta = %.2f, distance = %d",theta, distance));  
    delete img;  
  }    
  //距離が100以下であれば向きを変えます    
  if(distance < 100){    
    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;    
}    
}}


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

distanceSensor1D()で視線方向の水平面に沿った1次元の距離データを取得します。戻り値はViewImageクラスです。
デフォルトではカメラの水平方向の視野角を320等分したデータを取得します。距離データを取得するにはgetBuffer()を使います。0が横方向の視野角内のいちばん左、319がいちばん右のデータになります。


***視野角の設定 [#fdc0feb2]
カメラの水平方向視野角は直接設定することはできませんが、垂直方向視野角とアスペクト比により調節できます。

ただし、視野角が大きくなると解像度が悪くなるため、視野角(水平方向)30°～80°くらいの範囲に設定することをお勧めします。

***コンパイル [#ra011e6b]
コンパイルします。
 $ ./sigmake.sh distanceSensor1D.cpp


***世界ファイル修正 [#o2d976b2]
コントローラdistanceSensor1D.soに変えます。

 $ emacs distanceSensor.xml

#highlight(xml:firstline[13]){{
    <!--コントローラの指定-->
    <set-attr-value name="implementation"
                    value="./distanceSensor.so"/>
}}
　        ↓
#highlight(xml:firstline[13]){{
    <!--コントローラの指定-->
    <set-attr-value name="implementation"
                    value="./distanceSensor1D.so"/>
}}


***実行 [#o60b67bd]


実行してみましょう。
 $ sigserver.sh  -w ./distanceSensor.xml

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

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

distanceSensor1D.cppの以下を修正すると2次元距離データ取得が可能です。
#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]
それでは実行してみましょう。
 $ sigserver.sh -w distanceSensor.xml


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


#highlight(end)

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


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

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

     

Site admin: inamura

PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.
Based on "PukiWiki" 1.3 by yu-ji. Powered by PHP 5.5.9-1ubuntu4.29. HTML convert time: 0.007 sec.