Backup of Distance sensor(No. 5)

List of Backups
View the diff.
View the diff current.
View the source.
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: 眼球運動?　　　

Distance sensor
Old Version

Distance sensor †

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

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)

↑

1) scalar distance †

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

↑

Controller †

Move to the working directory and edit a controller.

$ 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:     
    
  //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 with a frequency    
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 ); 
    
  unsigned char distance = 255;  
  if(m_view != NULL) {  
    // Measure the distance
    distance = m_view->distanceSensor();    
    LOG_MSG(("distance = %d",distance));    
  }    
  // 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;    
}

The distanceSensor() in the 62nd line gets the distance.

The type of the return value is 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で取得した画像と同様に距離データをファイルに保存する場合は以下の関数を使います。

↑

世界ファイル修正 †

$ emacs distanceSensor.xml

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

↑

実行 †

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

$ sigserver.sh -w ./distanceSensor.xml

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

#ref(): File not found: "distance.bmp" at page "Distance sensor"

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

↑

Old Version †

距離センサ(v2.0系)?
距離センサ(v120330, v1.4.8)?

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

Backup of Distance sensor (No. 5)

Distance sensor †

1) scalar distance †

Controller †

世界ファイル †

実行 †

1次元距離データ取得 †

コントローラ修正 †

視野角の設定 †

コンパイル †

世界ファイル修正 †

実行 †

2次元距離データ取得 †

コントローラ †

世界ファイル修正 †

実行 †

Old Version †