Up:[[Tutorial]]    Previous:[[車輪移動ロボット]]    Next: [[眼球運動]]   
Up:[[Tutorial]]    Previous:[[Wheeled mobile robot]]    Next: [[Eye movement]]   

----
#contents

*距離センサ [#r0d5dd55]
*Distance sensor [#m5e8543a]

This tutorial is valid for version later than v2.1.0.

SIGVerseでは距離センサはカメラを用います。
ここではSIGViewerサービスから距離データを取得するサンプルを紹介します。
Distance sensor uses camera (depth image).
This page explains how to get distance information from SIGViewer service.

There are three types of distance information:
1) distance from the camera and a target object (scalar)
2) distance array measured along a horizontal plane (vector)
3) depth map (2D matrix)

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

This sample uses a function distanceSensor() to get a distance between the camera and a target object which is gazed by the camera.

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

**視線方向の距離センサ [#r07b5fe9]
distanceSensor()を使ってカメラの視線方向にあるオブジェクトまでの距離を取得するサンプルを紹介します。
***コントローラ作成 [#j5a63794]
まずはコントローラを作成します。
 $ cd sigverse-<version>/bin/NewWorld
Move to the working directory and edit a controller.
 $ 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;    
  double vel;      //velocity of a robot
  ViewService *m_view;  
};     
    
void RobotController::onInit(InitEvent &evt)    
{    
  m_view = (ViewService*)connectToService("SIGViewer", 9005);  
  m_view = (ViewService*)connectToService("SIGViewer");  
  vel      = 1.0;    
  srand(time(NULL));    
}      
    
//定期的に呼び出される関数    
//Callback function which is called at fixed intervals.
double RobotController::onAction(ActionEvent &evt)    
{     
  SimObj *my = getObj(myname());    
    
  //自分の位置を得ます
  // Get current self-position
  Vector3d pos;
  my->getPosition(pos);    
    
  //y軸周りの自分の回転を得ます(クオータニオン)    
  double qy = my->qy();    
  double qw = my->qw();    
  // Get current self-orientation    
  Rotation rot;
  my->getRotation(rot);
  double qy = rot.qy();    
  double qw = rot.qw();    
  double tmp = qy*qw;
  if(tmp < 0) qy = -qy;
  
  //クオータニオンから回転角を導出します    
  // Calculate angle from quaternion 
  double theta = 2*asin(qy);    

  double dx = 0;    
  double dz = 0;    
    
  //移動する方向を決定します  
  // Calculate moving direction
  dx = sin(theta) * vel;    
  dz = cos(theta) * vel;    
    
  //移動します    
  my->setPosition( pos.x() + dx, pos.y() , pos.z() + dz );    
  // Execution of move
  my->setPosition( pos.x() + dx, pos.y() , pos.z() + dz ); 
    
  unsigned char distance = 255;  
  if(m_view != NULL) {  
    //視線方向のオブジェクトまでの距離を取得します    
    // Measure the distance
    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);    
    }     
  // Change direction if the distance is shorter than 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()で距離データを取得しています。
The distanceSensor() in the 62nd line gets the distance.
#highlight(cpp:firstline[62]){{
      distance = m_view->distanceSensor();
}}
戻り値はunsigned char型です。つまり取得できる距離データは0~255までの整数です。デフォルトでは255cmまでの距離を取得することができ、255よりも遠くにオブジェクトがある場合はすべて255が返ってきます。
引数を変えることによってこれらの距離を変更することができます。
The type of the return value is unsigned char, that indicates from 0 to 255. Maximum distance is 255cm. If a true distance is longer than 255cm, the return value will be 255.
The maximum distance can be changed by argument of distanceSensor() as shown below:

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

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


change
#highlight(cpp:firstline[62]){{
      distance = m_view->distanceSensor();
}}
    ↓
to
#highlight(cpp:firstline[62]){{
     distance = m_view->distanceSensor(50.0, 500.0, 2);
}}
最初2つの引数で距離センサが取得できる距離の範囲を変更することができます。第一引数にoffset値、第2引数に取得する距離データの範囲を指定します。この例ではカメラから距離が50.0~550.0(cm)までの距離データを取得できることになります。このとき戻り値は先ほどと同様0~255です。つまりカメラからの距離が50.0cm以下の場合は戻り値が0になり、550.0cm以上の場合は255となります。取得できる距離データの値と実際の距離は常に比例関係にあります。範囲を大きくすると広範囲の距離データを取得することができますが分解能が悪くなります。
The first and second argument indicate range of the distance sensor. The first argument is offset value. The second argument is range of measurable area.
In the above example case, the distance sensor can measure from 50cm to 550cm.
The range of the return value is still from 0 to 255. If the distance is shorter than 50cm, the return value will be 0; if the distance is longer than 550cm, the return value will be 255.
Resolution of the distance sensor will be rough, if you choose wide range.

最後の引数はカメラID番号です。
何も指定しない場合はカメラのID番号1から距離データを取得します。
The last argument is ID number of camera. If you omit this argument, the distance sensor will be corresponded to the camera which ID is No.1.

このコントローラではロボットは向いている方向に進み、視線方向のオブジェクトまでの距離が80よりも近くなるとロボットの体全体の向きをランダムに変えます。
In this controller, a robot keeps on going forward; if the distance is shorter than 100cm, the direction will be changed randomly.

Makefileに作成したファイルを追加してコンパイルします。(手順省略)
Compile the source code of the controller.

 $ ./sigmake.sh distanceSensor.cpp

***世界ファイル [#md4f157f]
次に世界ファイルを作成します。
*** World File [#w89a6566]
Next, edit a world file

 $ cd ..
 $ emacs xml/distanceSensor.xml
 $ 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 the name of a robot agent -->
    <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"/>
    <!-- Setting of controller -->
    <set-attr-value name="implementation" value="./distanceSensor.so"/>

    <!--動力学演算をfalseに設定-->
    <!-- Put the flag of dynamics to false -->
    <set-attr-value name="dynamics" value="false"/>
    
    <!--エージェントの位置(x,y,z)-->
    <!-- Set the initial position of agent(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方向)の設定, 縦横比-->
    <!--Setting of camera parameters-->
    <camera id="1"
            link="HEAD_LINK"
            direction="0 0 1"
            position="0.0 0.0 5.0"
            fov="80"
            fov="45"
            aspectRatio="1.5"/>
  </instanciate>

  <!--リビングルーム-->
  <!--Living Room-->
  <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="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="80.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="32.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="8.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="72.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>
}}

今回は車輪付き移動ロボットを使用します。
This time, a wheeled mobile robot is used.


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

#ref(./distance_1.JPG,60%)
Execute with the following command

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

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

The robot will go forward; change the direction in front of obstacles.

**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();
** Measurement of distance vector [#l8f8546a]

    //データの長さを取得します 
    int length = img->getBufferLength();
Next, an example that measure distance vector along a horizontal plane.

    //水平面内の距離データの最小値を求めます
    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;
*** Modification of Controller [#nf3fb7da]

Edit another new controller

 $ 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:     
    
  //velocity of a robot
  double vel;    
  ViewService *m_view;  
};     
    
void RobotController::onInit(InitEvent &evt)    
{    
  m_view = (ViewService*)connectToService("SIGViewer");  
  vel      = 1.0;    
  srand(time(NULL));    
}      
    
//Callback function which is called at fixed intervals.
double RobotController::onAction(ActionEvent &evt)    
{     
  SimObj *my = getObj(myname());    
    
  // Get current self-position
  Vector3d pos;
  my->getPosition(pos);    
    
  // Get current self-orientation 
  Rotation rot;
  my->getRotation(rot);
  double qy = rot.qy();    
  double qw = rot.qw();

  double tmp = qy*qw;
  if(tmp < 0) qy = -qy;
  
  // Calculate angle from quaternion     
  double theta = 2*asin(qy);    

  double dx = 0;    
  double dz = 0;    
    
  // Calculate moving direction 
  dx = sin(theta) * vel;    
  dz = cos(theta) * vel;    
    
  // Execution of move 
  my->setPosition( pos.x() + dx, pos.y() , pos.z() + dz );    
  
  // Calculate vertical angle view (radian) of a camera.
  double fovy = my->getCamFOV() * PI / 180.0;

  // Get aspect ratio
  double ar = my->getCamAS();
  
  // Calculate horizontal angle view (degree) of a camera.
  double fovx = 2 * atan(tan(fovy*0.5)*ar) * 180.0 / PI;

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

    // Get distance vector along a horizontal plane
    ViewImage *img = m_view->distanceSensor1D();  
    char *buf = img->getBuffer();  
  
    // Get length of the data array
    int length = img->getBufferLength();  

    double theta = 0.0;
  
    // Calculate minimum distance value in the array
    for(int i = 0; i < length; i++){  
      unsigned char tmp_distance = (unsigned char)buf[i];  
      if(tmp_distance < distance){  
	// Calculate view angle from index of an array
	theta = fovx*i/319.0 - fovx/2.0;
	distance = buf[i];  
      }  
    }  
    LOG_MSG(("theta = %.1f, distance = %d",theta, distance));  
    delete img;  
  }    
  // Change direction if the distance is shorter than 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;    
}    
}}
distanceSensor1D()で視線方向の水平面に沿った1次元の距離データを取得します。戻り値はViewImageクラスです。
デフォルトではカメラの視野角内の幅320ピクセルのデータが取得します。距離データを取得するにはgetBuffer()を使います。0が横方向の視野角内のいちばん左、320がいちばん右のピクセルになります。

このサンプルは水平面の距離データの最小値(いちばん近い距離)が80cm以下の場合に方向を変えるサンプルです。
In this example, the robot changes direction when the shortest distance in the distance vector is shorter than 100cm.

コンパイルします。
 $ make
distanceSensor1D() gets the distance vector along a horizontal plane which position is identical to a target camera.
Class of the return value is ViewImage.

The range of the measurable angle is identical to the horizontal angle of camera view.
The size of the vector is 320 in initial setting; each array element is corresponded to each pixel.
getBuffer() should be used to get each distance value from the array. array[0] corresponds to the leftmost pixel; array[319] corresponds to the rightmost pixel.

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

先ほどのサンプルでは近くにオブジェクトがあっても視線方向と外れていたら向きを変えませんでした。
このサンプルでは近くにオブジェクトがあれば向きを変えます。
4つのオブジェクトに囲まれたロボットはなかなか外に出られなくなります。
You can change the horizontal view angle with vertical view angle and aspect ratio in a world file.

**2次元距離センサ [#z93a6d1d]
次は視野全体の距離データを取得します。
***コントローラ [#n495b830]
Due to the resolution quality, the recommended horizontal angle is from 30(deg) to 80(deg).

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

Enter the following command to compile.

 $ ./sigmake.sh distanceSensor1D.cpp


*** Edit of the world file [#qa3ffb5d]

Change the controller to distanceSensor1D.so

 $ emacs distanceSensor.xml

#highlight(xml:firstline[13]){{
    <!--assignment of controller-->
    <set-attr-value name="implementation"
                    value="./distanceSensor.so"/>
}}
         ↓
#highlight(cpp:firstline[61]){{
    //視野全体の距離データを取得します
    ViewImage *img = m_view->distanceSensor2D();
#highlight(xml:firstline[13]){{
    <!--assignment of controller-->
    <set-attr-value name="implementation"
                    value="./distanceSensor1D.so"/>
}}


*** Execution [#m3c8b021]

captureViewで取得した画像と同様に距離データをファイルに保存したい場合は以下の行を適当に追加します。
#highlight(cpp:firstline[81]){{
Enter the following command to execute.

 $ sigserver.sh  -w ./distanceSensor.xml

In this example, quality of the avoidance behavior becomes better.
The robot cannot escape from the four obstacles.


** Measurement of 2D matrix distance [#c26b360c]

This section explains how to get the 2D matrix distance.

*** Edit of controller [#v5b64855]

Edit another new controller.

 $ emacs distanceSensor2D.cpp

distanceSensor2D.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:     
    
  // velocity of a robot
  double vel;    
  ViewService *m_view;  
};     
    
void RobotController::onInit(InitEvent &evt)    
{    
  m_view = (ViewService*)connectToService("SIGViewer");  
  vel      = 2.0;    
  srand(time(NULL));    
}      
    
//Callback function which is called at fixed intervals.
double RobotController::onAction(ActionEvent &evt)    
{     
  SimObj *my = getObj(myname());    
    
  // Get current self-position
  Vector3d pos;
  my->getPosition(pos);    
    
  // Get current self-orientation
  Rotation rot;
  my->getRotation(rot);
  double qy = rot.qy();    
  double qw = rot.qw();
 
  double tmp = qy*qw;
  if(tmp < 0) qy = -qy;
  
  // Calculate angle from quaternion
  double theta = 2*asin(qy);    

  double dx = 0;    
  double dz = 0;    
    
  // Calculate moving direction
  dx = sin(theta) * vel;    
  dz = cos(theta) * vel;    
    
  // Execution of move
  my->setPosition( pos.x() + dx, pos.y() , pos.z() + dz );    
  
  // Get vertical view angle of the camera (rad)
  double fovy = my->getCamFOV() * PI / 180.0;

  // Calculate aspect ratio
  double ar = my->getCamAS();
  
  // Calculate horizontal view angle phi(deg)
  double fovx = 2 * atan(tan(fovy*0.5)*ar) * 180.0 / PI;

  // Calculate vertical view angle phi(deg)
  fovy = fovy * 180.0 / PI;

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

    // Gets the distance matrix
    ViewImage *img = m_view->distanceSensor2D();  
    char *buf = img->getBuffer();  
  
    // Width of the distance matrix
    int width = img->getWidth();

    // Height of the distance matrix
    int height = img->getHeight();

    // Horizontal angle (origin is direction of a camera)
    double phi = 0.0;

    // Vertical angle (origin is direction of a camera)
    double theta = 0.0;
  
    // Calculate of minimum distance in the distance matrix
    for(int i = 0; i < width; i++){  
      for(int j = 0; j < height; j++){
	int index = j *width + i;
	unsigned char tmp_distance = (unsigned char)buf[index];
	if(tmp_distance < distance){

	  // Calculate horizontal view angle phi(rad) from a index of pixel.
	  phi   = fovx*i/(width-1.0) - fovx/2.0;
	  
	  // Calculate vertical view angle theta(rad) from a index of pixel.
	  theta = fovy*j/(height-1.0) - fovy/2.0;
	  
	  distance = buf[index];  
	}
      }  
    }  
    LOG_MSG(("phi = %.1f, theta = %.1f, distance = %d",phi, theta, distance));  

    // Chage the orientation if the distance is less than 100
    if(distance < 100){    
      my->setAxisAndAngle(0.0, 1.0, 0.0, (double)rand()/RAND_MAX * 2*PI, true);    
      // Save a range imagery at this moment
      img->saveAsWindowsBMP("distance.bmp");
    }     
    delete img;  
  }    
  return 0.2;
}     
    
extern "C" Controller * createController ()    
{    
  return new RobotController;    
}    

}}

distanceSensor2D() gets the distance matrix.


In this sample, theta (vertical angle) and phi (horizontal angle) in polar coordinate are calculated from pixel position.


Use the following function to save the depth map image as well as captureView().

#highlight(cpp:firstline[113]){{
 img->saveAsWindowsBMP("distance.bmp");
}}

*** Edit of world file [#ie0b1fde]

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

Change the controller from distanceSensor1D.so to distanceSensor2D.so.

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

Enter the following command to execute.

 $ sigserver.sh -w ./distanceSensor.xml


The depth map image will be created as shown below:

#ref(distance.jpg)


#highlight(end)

遠いオブジェクトほど色が薄くなり、近いオブジェクトほど色が濃くなっています。
Further distance is shown by dark; shorter distance is shown by light.


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

Up:[[Tutorial]]    Previous:[[車輪移動ロボット]]    Next: [[眼球運動]]   
Up:[[Tutorial]]    Previous:[[Wheeled mobile robot]]    Next: [[Eye movement]]   
#counter


Front page   New List of pages Search Recent changes   Help   RSS of recent changes