- The added line is THIS COLOR.
- The deleted line is THIS COLOR.
Up:[[Tutorial]] Previous:[[車輪移動ロボット]] Next: [[眼球運動]]
#contents
*距離センサ [#r0d5dd55]
SIGVerseでは距離センサはカメラを用います。
ここではSIGViewerサービスから距離データを取得するサンプルを紹介します。
カメラから取得可能な距離データは3つあります。一つ目はカメラから視線方向にあるオブジェクトまでの点と点の距離(スカラ値)、2つ目はエージェントの視線から水平面に沿って,物体までの距離を連続的に得るベクトル値(一次元配列)、3つ目は視野全体に渡る,二次元配列の距離データです。
※このサンプルはサービスプロバイダ機能を使用するため、SSH接続の場合は追加でポートフォワーディングの設定が必要です。
**視線方向の距離センサ [#r07b5fe9]
distanceSensor()を使ってカメラの視線方向にあるオブジェクトまでの距離を取得するサンプルを紹介します。
***コントローラ作成 [#j5a63794]
まずはコントローラを作成します。
$ cd sigverse-<version>/bin/NewWorld
$ 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>
#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", 9005);
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;
#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", 9005);
vel = 10.0;
srand(time(NULL));
//クオータニオンから回転角を導出します
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));
}
//距離が80以下であれば向きを変えます
if(distance < 80){
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;
}
//定期的に呼び出される関数
double RobotController::onAction(ActionEvent &evt)
{
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 = 255;
if(m_view != NULL) {
//視線方向のオブジェクトまでの距離を取得します
distance = m_view->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);
}
return 1.0;
}
extern "C" Controller * createController ()
{
return new RobotController;
}
}}
このコントローラは61行目のdistanceSensor()で距離データを取得しています。
#highlight(cpp:firstline[61]){{
このサンプルでは62行目のdistanceSensor()で距離データを取得しています。
#highlight(cpp:firstline[62]){{
distance = m_view->distanceSensor();
}}
戻り値はunsigned char型です。つまり取得できる距離データは0~255までの整数です。デフォルトでは255cmまでの距離を取得することができ、255よりも遠くにオブジェクトがある場合はすべて255が返ってきます。
引数を変えることによってこれらの距離を変更することができます。
さらに遠くの距離を取得したい場合などはdistanceSensor()の引数に取得したい距離データの範囲を指定することができます。
より遠くの距離を取得したい場合はdistanceSensor()の引数に取得したい距離データの範囲を指定することができます。
distanceSensor()を例えば以下のように修正します。
#highlight(cpp:firstline[61]){{
#highlight(cpp:firstline[62]){{
distance = m_view->distanceSensor();
}}
↓
#highlight(cpp:firstline[61]){{
#highlight(cpp:firstline[62]){{
distance = m_view->distanceSensor(50.0, 500.0, 2);
}}
最初2つの引数で距離センサが取得できる距離の範囲を指定します。この例ではカメラから距離が50.0~500.0(cm)までの距離データを取得できることになります。このとき戻り値は先ほどと同様0~255です。つまりカメラからの距離が50.0cm以下の場合は戻り値が0になり、500.0cm以上の場合は255となります。取得できる距離データの値と実際の距離は常に比例関係にあります。範囲を大きくすると広範囲の距離データを取得することができますが分解能が悪くなります。
最初2つの引数で距離センサが取得できる距離の範囲を変更することができます。第一引数にoffset値、第2引数に取得する距離データの範囲を指定します。この例ではカメラから距離が50.0~550.0(cm)までの距離データを取得できることになります。このとき戻り値は先ほどと同様0~255です。つまりカメラからの距離が50.0cm以下の場合は戻り値が0になり、550.0cm以上の場合は255となります。取得できる距離データの値と実際の距離は常に比例関係にあります。範囲を大きくすると広範囲の距離データを取得することができますが分解能が悪くなります。
最後の引数はカメラID番号です。
何も指定しない場合はカメラのID番号1から距離データを取得します。
このコントローラではロボットは向いている方向に進み、視線方向のオブジェクトまでの距離が120よりも近くなるとロボットの体全体の向きをランダムに変えます。
このコントローラではロボットは向いている方向に進み、視線方向のオブジェクトまでの距離が80よりも近くなるとロボットの体全体の向きをランダムに変えます。
Makefileに作成したファイルを追加してコンパイルします。(手順省略)
***世界ファイル [#md4f157f]
次に世界ファイルを作成します。
$ cd ..
$ emacs xml/distanceSensor.xml
distanceSensor.xml
#highlight(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"/>
<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="./NewWorld/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="60.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="qx" 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="80"
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="62.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="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="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"/>
<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"/>
</instanciate>
<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 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="z" value="100.0"/>
<set-attr-value name="qw" value="0.0"/>
<set-attr-value name="qy" value="1.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>
<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="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="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 -p 9001 -w xml/distanceSensor.xml
#ref(distance_1.jpg)
#ref(./distance_1.JPG,60%)
シミュレーションを実行してみるとロボットが進んで、オブジェクトに近づくとロボットが方向転換しているのがわかると思います。
シミュレーションを実行するとロボットが前に進み始めます。そしてオブジェクトに衝突しそうになると方向転換します。
**距離範囲、カメラIDの指定 [#baeff175]
デフォルトでは0~255(SIGVerseの距離)までの距離データしか取得することができません。
さらにカメラが複数ある場合などは3番目の引数でカメラIDを指定します。
この後で出てくるdistanceSensor1D, distanceSensor2Dも同様の設定が可能です。
**1次元距離センサ [#cb2b5721]
エージェントの視線の水平面に沿って距離を取得するサンプルです。水平面の距離データが配列データで一度に取得することができます。
次にエージェントの視線方向の水平面に沿って距離データを取得するサンプルを紹介します。水平面の距離データが配列データとして一度に取得します。
***コントローラ修正 [#s7488bb0]
コントローラを修正します。
$ cd NewWorld
$ emacs -nw distanceSensor.cpp
distanceSensor.cpp
#highlight(cpp:firstline[59]){{
//視線方向のオブジェクトまでの距離を取得します。
unsigned char = distanceSensor();
#highlight(cpp:firstline[61]){{
//視線方向のオブジェクトまでの距離を取得します
distance = m_view->distanceSensor();
LOG_MSG(("distance = %d",distance));
}}
↓
#highlight(cpp:firstline[59]){{
//水平面の深度データを取得します。
DepthImage *img = distanceSensor1D();
double distance = 255.0;
//水平面内の距離データの最小値を求めます。
for(int i = 0; i < 320; i++){
//ピクセルの位置を指定し距離データを取得します。
double tmp_dis = img->getDistanceFromPixel(i);
if(distance > tmp_dis) {
distance = tmp_dis;
#highlight(cpp:firstline[61]){{
//視線方向水平面の距離データを取得します
ViewImage *img = m_view->distanceSensor1D();
char *buf = img->getBuffer();
//データの長さを取得します
int length = img->getBufferLength();
//水平面内の距離データの最小値を求めます
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 = %.1f",distance));
LOG_MSG(("distance = %d",distance));
delete img;
}}
distanceSensor1D()で視線方向の水平面に沿った1次元の深度マップが得られます。戻り値はクラスDepthImageです。
デフォルトではカメラの視野角内の幅320ピクセル(高さ1ピクセル)のデータが取得できます。(2011年9月の時点では320のみ対応)。距離データを取得するにはgetDistanceFromPixel()を使います。引数はピクセル位置を指定します。0が横方向の視野角内のいちばん左、320がいちばん右のピクセルになります。
distanceSensor1D()で視線方向の水平面に沿った1次元の距離データを取得します。戻り値はViewImageクラスです。
デフォルトではカメラの視野角内の幅320ピクセルのデータが取得します。距離データを取得するにはgetBuffer()を使います。0が横方向の視野角内のいちばん左、320がいちばん右のピクセルになります。
//pixelではなくカメラからの方向を指定して距離データを取得する場合は//getDepthFromAngle()を使います。このとき引数で方向を指定します。単位はdegree、 //視線方向が0、左が+。(2次元距離データの場合は2番目の引数に高さ方向の角度指定)
このサンプルは水平面の距離データの最小値(いちばん近い距離)が80cm以下の場合に方向を変えるサンプルです。
このサンプルは水平面の距離データの最小値(いちばん近い距離)が120以下の場合に方向を変えるサンプルです。
コンパイルします。
$ make
***実行 [#o60b67bd]
実行してみましょう。
$ cd ../
$ ./sigserver.sh -p 9001 -w xml/distanceSensor.xml
先ほどのサンプルでは近くにオブジェクトがあっても視線方向と外れていたら向きを変えませんでしたが、
こちらのサンプルではロボットはより衝突を回避できているのがわかります。
先ほどのサンプルでは近くにオブジェクトがあっても視線方向と外れていたら向きを変えませんでした。
このサンプルでは近くにオブジェクトがあれば向きを変えます。
4つのオブジェクトに囲まれたロボットはなかなか外に出られなくなります。
**2次元距離センサ [#z93a6d1d]
視野全体の距離データを取得するサンプルを紹介します。
次は視野全体の距離データを取得します。
***コントローラ [#n495b830]
新しくコントローラを作成します。
$ cd NewWorld
$ emacs distanceSensor2D.cpp
distanceSensor2D.cpp
#highlight(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;
}
distanceSensor.cppの以下を修正します。
#highlight(cpp:firstline[61]){{
//視線方向水平面の距離データを取得します
ViewImage *img = m_view->distanceSensor1D();
}}
このサンプルはcaptureViewのときと同様にエージェントに"move","rotation"等のメッセージを送信してロボットを操作します。また、"capture"を送信した場合はdistanceSensor2D()を使って視野全体の距離データ(深度画像)を取得し、その中で最も近いオブジェクトがある方向を計算し、その方向をロボットが指さします。画像中に同じ距離が複数ある場合は左上を優先して指さします。デフォルトで取得できる距離データのサイズは320×240(2011年9月の時点では320×240のみ対応)です。
また、captureViewで取得した画像と同様に
#highlight(cpp:firstline[97]:nocontrols){{
img->saveAsWindowsBMP(fname);
↓
#highlight(cpp:firstline[61]){{
//視野全体の距離データを取得します
ViewImage *img = m_view->distanceSensor2D();
}}
で距離データを保存し、可視化することができます。
それではMakefileを修正してコンパイルします。
$ make
***世界ファイルの編集 [#lfddf4ed]
次に世界ファイルを修正します。
$ cd ..
$ emacs xml/distanceSensor.xml
コントローラ指定箇所を修正します。
distanceSensor.xml
#highlight(xml:firstline[40]){{
<set-attr-value name="implementation" value="./distanceSensor.so"/>
captureViewで取得した画像と同様に距離データをファイルに保存したい場合は以下の行を適当に追加します。
#highlight(cpp:firstline[81]){{
img->saveAsWindowsBMP("distance.bmp");
}}
↓
#highlight(xml:firstline[40]){{
<set-attr-value name="implementation" value="./distanceSensor2D.so"/>
}}
視線ベクトルを修正します。(※下を向いていると地面を検出してしまうので。。)
#highlight(xml:firstline[25]){{
<set-attr-value name="evx1" value="0.0"/>
<set-attr-value name="evy1" value="-1.0"/>
<set-attr-value name="evz1" value="1.0"/>
}}
↓
#highlight(xml:firstline[25]){{
<set-attr-value name="evx1" value="0.0"/>
<set-attr-value name="evy1" value="0.0"/>
<set-attr-value name="evz1" value="1.0"/>
}}
***実行 [#taae64c5]
それでは実行してみましょう。
$ cd ~/sigverse/bin
$ ./sigserver.sh -p 9001 -w xml/distanceSensor.xml
ロボットがリビングに立っているので好きなところでメッセージ"capture"を送信して見てください。ロボットが最も近いオブジェクトの方向を指します。
保存した距離データ画像を見てみると以下のような距離データが見ることができます。
#ref(distance.bmp)
#ref(distance_2.jpg)
次に保存した深度画像を見てみましょう。
以下のような濃淡画像が見えると思います。
#ref(view001.jpg)
深度画像の例
#highlight(end)
遠いオブジェクトほど色が薄くなり、近いオブジェクトほど色が濃くなっています。
*Old Version [#gadb4153]
-[[距離センサ(v120330, v1.4.8)]]
Up:[[Tutorial]] Previous:[[車輪移動ロボット]] Next: [[眼球運動]]