距離センサ

Up:Tutorial?　　　Previous：車両の制御（動力学有り）　　　 Next: 眼球運動

距離センサ
Old Version

距離センサ †

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

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

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

↑

視線方向の距離センサ †

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

↑

コントローラ作成 †

作業ディレクトリに移動しコントローラを作成します。

$ cd ~/MyWorld
$ 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;      //移動速度
  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);    
    
  //自分の回転を得ます（クオータニオン）    
  Rotation rot;
  my->getRotation(rot);
  double qy = rot.qy();    
  double qw = rot.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()で距離データを取得しています。

戻り値はunsigned char型です。つまり取得できる距離データは0～255までの整数です。デフォルトでは255cmまでの距離を取得することができ、255よりも遠くにオブジェクトがある場合はすべて255が返ってきます。引数を変えることによってこれらの距離を変更することができます。

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

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

　　　　↓

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

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

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

↑

コンパイル †

コンパイルします。

$ ./sigmake.sh distanceSensor.cpp

↑

世界ファイル †

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

$ emacs distanceSensor.xml

distanceSensor.xml

<?xml version="1.0" encoding="utf8"?>
<world name="myworld5">

<set-attr-value name="name" value="robot_000"/>

<set-attr-value name="language" value="c++"/>

<set-attr-value name="implementation"
                    value="./distanceSensor.so"/>

<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="-40.0"/>
    
    <set-attr-value name="qw" value="0.0"/>
    <set-attr-value name="qy" value="1.0"/>
    
    
    <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>

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

↑

実行 †

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

$ sigserver.sh -w ./distanceSensor.xml

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

↑

1次元距離データ取得 †

次にエージェントの視線方向の水平面に沿って距離データを取得するサンプルを紹介します。水平面の距離データを配列データとして一度に取得します。

↑

コントローラ修正 †

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

$ emacs distanceSensor1D.cpp

distanceSensor1D.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);    
    
  //自分の回転を得ます（クオータニオン）    
  Rotation rot;
  my->getRotation(rot);
  double qy = rot.qy();    
  double qw = rot.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 = %.1f, 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がいちばん右のデータになります。

↑

視野角の設定 †

カメラの水平方向視野角は直接設定することはできませんが、垂直方向視野角とアスペクト比を世界ファイルで設定することにより調節できます。

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

↑

コンパイル †

コンパイルします。

$ ./sigmake.sh distanceSensor1D.cpp

↑

世界ファイル修正 †

コントローラdistanceSensor1D.soに変えます。

$ emacs distanceSensor.xml

　 ↓

↑

実行 †

実行してみましょう。

$ sigserver.sh  -w ./distanceSensor.xml

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

↑

2次元距離データ取得 †

次は視野全体の距離データを取得します。

↑

コントローラ †

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

$ emacs distanceSensor2D.cpp

distanceSensor2D.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      = 2.0;    
  srand(time(NULL));    
}      
    
//定期的に呼び出される関数    
double RobotController::onAction(ActionEvent &evt)    
{     
  SimObj *my = getObj(myname());    
    
  //自分の位置を得ます
  Vector3d pos;
  my->getPosition(pos);    
    
  //自分の回転を得ます（クオータニオン）    
  Rotation rot;
  my->getRotation(rot);
  double qy = rot.qy();    
  double qw = rot.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 );    
  
  // カメラの垂直方向の視野角(rad)を取得します
  double fovy = my->getCamFOV() * PI / 180.0;

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

// カメラの垂直方向の視野角(deg)を計算します
  fovy = fovy * 180.0 / PI;

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

// 視線方向水平面の距離データを取得します  
    ViewImage *img = m_view->distanceSensor2D();  
    char *buf = img->getBuffer();  
  
    // 距離データ画像の幅取得
    int width = img->getWidth();

// 距離データ画像の高さ取得
    int height = img->getHeight();

// 視線方向からの水平面の角度
    double phi = 0.0;

// 視線方向からの垂直方向の角度
    double theta = 0.0;
  
    // 距離データの最小値を求めます  
    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){

// 水平方向における視線方向からの角度(rad)を計算します
	  phi   = fovx*i/(width-1.0) - fovx/2.0;
	  
	  // 垂直方向における視線方向からの角度(rad)を計算します
	  theta = fovy*j/(height-1.0) - fovy/2.0;
	  
	  distance = buf[index];  
	}
      }  
    }  
    LOG_MSG(("phi = %.1f, theta = %.1f, distance = %d",phi, theta, distance));

//距離が100以下であれば向きを変えます    
    if(distance < 100){    
      my->setAxisAndAngle(0.0, 1.0, 0.0, (double)rand()/RAND_MAX * 2*PI, true);    
      // 向きを変える瞬間の距離画像を保存します
      img->saveAsWindowsBMP("distance.bmp");
    }     
    delete img;  
  }    
  return 0.2;
}     
    
extern "C" Controller * createController ()    
{    
  return new RobotController;    
}

distanceSensor2D()により2次元距離データを取得します。

取得した距離データの配列位置からその方向を極座標系で得ることができます。このサンプルでは方向をカメラ視線方向を基準に垂直方向の角度Θと水平方向の角度φで求めています。

captureViewで取得した画像と同様に距離データをファイルに保存する場合は以下の関数を使います。

↑

コンパイル †

コンパイルします。

$ ./sigmake.sh distanceSensor2D.cpp

↑

世界ファイル修正 †

$ emacs distanceSensor.xml

コントローラをdistanceSensor1D.soからdistanceSensor2D.soに変えます。

↑

実行 †

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

$ sigserver.sh -w ./distanceSensor.xml

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

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

↑

Old Version †

Up:Tutorial?　　　Previous：車両の制御（動力学有り）　　　 Next: 眼球運動

Counter: 3339, today: 1, yesterday: 0

recent(20)