距離センサ(v120330, v1.4.8)
[
Front page
] [
New
|
List of pages
|
Search
|
Recent changes
|
Help
]
Start:
Up:[[Tutorial]] Previous:[[車輪移動ロボット]] ...
#contents
*距離センサ [#r0d5dd55]
ここでは距離センサ機能を使ったサンプルを紹介します。距離...
※このサンプルはSIGVerseのバージョン110927, SIGViewer-1.4....
※このサンプルはサービスプロバイダ機能を使用するため、ポー...
**視線方向の距離センサ [#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>
#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;
};
void RobotController::onInit(InitEvent &evt)
{
vel = 10.0;
srand(time(NULL));
}
//定期的に呼び出される関数
double RobotController::onAction(ActionEvent &evt)
{
try {
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 = distanceSensor();
LOG_MSG(("distance = %d",distance));
//距離が120以下であれば向きを変えます
if(distance < 120)
{
my->setAxisAndAngle(0.0, 1.0, 0.0, (double)rand(...
}
} catch (SimObj::Exception &) {
}
return 1.0;
}
extern "C" Controller * createController ()
{
return new RobotController;
}
}}
このコントローラは以下の行
#highlight(cpp:firstline[60]){{
unsigned char distance = distanceSensor();
}}
で視線方向のオブジェクトまでの距離を取得しています。戻り...
このコントローラではロボットは向いている方向に進み、視線...
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"/>
<set-attr-value name="dynamics" value="false"/>
<set-attr-value name="x" value="0.0"/>
<set-attr-value name="y" value="60.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"/>
<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"/>
<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="./NewWor...
</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>
<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>
</world>
}}
***実行 [#l3f6882d]
それでは実行してみましょう。
$ ./sigserver.sh -p 9001 -w xml/distanceSensor.xml
#ref(distance_1.jpg)
シミュレーションを実行してみるとロボットが進んで、オブジ...
**距離範囲、カメラIDの指定 [#baeff175]
デフォルトでは0~255(SIGVerseの距離)までの距離データしか...
さらに遠くの距離を取得したい場合などはdistanceSensor()の...
distanceSensor()を例えば以下のように修正します。
#highlight(cpp:firstline[60]){{
unsigned char distance = distanceSensor();
}}
↓
#highlight(cpp:firstline[60]){{
unsigned char distance = distanceSensor(50.0, 500.0,...
}}
最初2つの引数で距離センサが取得できる距離の範囲を指定し...
さらにカメラが複数ある場合などは3番目の引数でカメラIDを指...
この後で出てくるdistanceSensor1D, distanceSensor2Dも同様...
**1次元距離センサ [#cb2b5721]
エージェントの視線の水平面に沿って距離を取得するサンプル...
***コントローラ修正 [#s7488bb0]
コントローラを修正します。
$ cd NewWorld
$ emacs -nw distanceSensor.cpp
distanceSensor.cpp
#highlight(cpp:firstline[59]){{
//視線方向のオブジェクトまでの距離を取得します。
unsigned char = 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;
}
}
LOG_MSG(("distance = %.1f",distance));
}}
distanceSensor1D()で視線方向の水平面に沿った1次元の深度マ...
デフォルトではカメラの視野角内の幅320ピクセル(高さ1ピク...
//pixelではなくカメラからの方向を指定して距離データを取得...
このサンプルは水平面の距離データの最小値(いちばん近い距...
コンパイルします。
$ make
***実行 [#o60b67bd]
実行してみましょう。
$ cd ../
$ ./sigserver.sh -p 9001 -w xml/distanceSensor.xml
先ほどのサンプルでは近くにオブジェクトがあっても視線方向...
こちらのサンプルではロボットはより衝突を回避できているの...
**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(fo...
if (img)
{
for(int i = 0; i < height; i++){
for(int j = 0; j < width; j++){
//ピクセル位置を指定して距離データ取得
double tmp_dis = img->getDistanceFrom...
if(distance > tmp_dis){
distance = tmp_dis;
//最小距離の時のpixel位置を取得します。
min_i = i;
min_j = j;
}
}
}
//最も近い物体までの距離を表示
LOG_MSG(("distance = %.1f", distance));
//最も近いオブジェクトの方向を計算し手を伸...
my->setJointAngle("RARM_JOINT2", atan(((hei...
my->setJointAngle("RARM_JOINT0", DEG2RAD(-9...
//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;
}
}}
このサンプルはcaptureViewのときと同様にエージェントに"mov...
また、captureViewで取得した画像と同様に
#highlight(cpp:firstline[97]:nocontrols){{
img->saveAsWindowsBMP(fname);
}}
で距離データを保存し、可視化することができます。
それではMakefileを修正してコンパイルします。
$ make
***世界ファイルの編集 [#lfddf4ed]
次に世界ファイルを修正します。
$ cd ..
$ emacs xml/distanceSensor.xml
コントローラ指定箇所を修正します。
distanceSensor.xml
#highlight(xml:firstline[40]){{
<set-attr-value name="implementation" value="./distanceS...
}}
↓
#highlight(xml:firstline[40]){{
<set-attr-value name="implementation" value="./distanceS...
}}
視線ベクトルを修正します。(※下を向いていると地面を検出し...
#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
ロボットがリビングに立っているので好きなところでメッセー...
#ref(distance_2.jpg)
次に保存した深度画像を見てみましょう。
以下のような濃淡画像が見えると思います。
#ref(view001.jpg)
深度画像の例
#highlight(end)
遠いオブジェクトほど色が薄くなり、近いオブジェクトほど色...
Up:[[Tutorial]] Previous:[[車輪移動ロボット]] ...
End:
Up:[[Tutorial]] Previous:[[車輪移動ロボット]] ...
#contents
*距離センサ [#r0d5dd55]
ここでは距離センサ機能を使ったサンプルを紹介します。距離...
※このサンプルはSIGVerseのバージョン110927, SIGViewer-1.4....
※このサンプルはサービスプロバイダ機能を使用するため、ポー...
**視線方向の距離センサ [#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>
#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;
};
void RobotController::onInit(InitEvent &evt)
{
vel = 10.0;
srand(time(NULL));
}
//定期的に呼び出される関数
double RobotController::onAction(ActionEvent &evt)
{
try {
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 = distanceSensor();
LOG_MSG(("distance = %d",distance));
//距離が120以下であれば向きを変えます
if(distance < 120)
{
my->setAxisAndAngle(0.0, 1.0, 0.0, (double)rand(...
}
} catch (SimObj::Exception &) {
}
return 1.0;
}
extern "C" Controller * createController ()
{
return new RobotController;
}
}}
このコントローラは以下の行
#highlight(cpp:firstline[60]){{
unsigned char distance = distanceSensor();
}}
で視線方向のオブジェクトまでの距離を取得しています。戻り...
このコントローラではロボットは向いている方向に進み、視線...
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"/>
<set-attr-value name="dynamics" value="false"/>
<set-attr-value name="x" value="0.0"/>
<set-attr-value name="y" value="60.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"/>
<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"/>
<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="./NewWor...
</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>
<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>
</world>
}}
***実行 [#l3f6882d]
それでは実行してみましょう。
$ ./sigserver.sh -p 9001 -w xml/distanceSensor.xml
#ref(distance_1.jpg)
シミュレーションを実行してみるとロボットが進んで、オブジ...
**距離範囲、カメラIDの指定 [#baeff175]
デフォルトでは0~255(SIGVerseの距離)までの距離データしか...
さらに遠くの距離を取得したい場合などはdistanceSensor()の...
distanceSensor()を例えば以下のように修正します。
#highlight(cpp:firstline[60]){{
unsigned char distance = distanceSensor();
}}
↓
#highlight(cpp:firstline[60]){{
unsigned char distance = distanceSensor(50.0, 500.0,...
}}
最初2つの引数で距離センサが取得できる距離の範囲を指定し...
さらにカメラが複数ある場合などは3番目の引数でカメラIDを指...
この後で出てくるdistanceSensor1D, distanceSensor2Dも同様...
**1次元距離センサ [#cb2b5721]
エージェントの視線の水平面に沿って距離を取得するサンプル...
***コントローラ修正 [#s7488bb0]
コントローラを修正します。
$ cd NewWorld
$ emacs -nw distanceSensor.cpp
distanceSensor.cpp
#highlight(cpp:firstline[59]){{
//視線方向のオブジェクトまでの距離を取得します。
unsigned char = 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;
}
}
LOG_MSG(("distance = %.1f",distance));
}}
distanceSensor1D()で視線方向の水平面に沿った1次元の深度マ...
デフォルトではカメラの視野角内の幅320ピクセル(高さ1ピク...
//pixelではなくカメラからの方向を指定して距離データを取得...
このサンプルは水平面の距離データの最小値(いちばん近い距...
コンパイルします。
$ make
***実行 [#o60b67bd]
実行してみましょう。
$ cd ../
$ ./sigserver.sh -p 9001 -w xml/distanceSensor.xml
先ほどのサンプルでは近くにオブジェクトがあっても視線方向...
こちらのサンプルではロボットはより衝突を回避できているの...
**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(fo...
if (img)
{
for(int i = 0; i < height; i++){
for(int j = 0; j < width; j++){
//ピクセル位置を指定して距離データ取得
double tmp_dis = img->getDistanceFrom...
if(distance > tmp_dis){
distance = tmp_dis;
//最小距離の時のpixel位置を取得します。
min_i = i;
min_j = j;
}
}
}
//最も近い物体までの距離を表示
LOG_MSG(("distance = %.1f", distance));
//最も近いオブジェクトの方向を計算し手を伸...
my->setJointAngle("RARM_JOINT2", atan(((hei...
my->setJointAngle("RARM_JOINT0", DEG2RAD(-9...
//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;
}
}}
このサンプルはcaptureViewのときと同様にエージェントに"mov...
また、captureViewで取得した画像と同様に
#highlight(cpp:firstline[97]:nocontrols){{
img->saveAsWindowsBMP(fname);
}}
で距離データを保存し、可視化することができます。
それではMakefileを修正してコンパイルします。
$ make
***世界ファイルの編集 [#lfddf4ed]
次に世界ファイルを修正します。
$ cd ..
$ emacs xml/distanceSensor.xml
コントローラ指定箇所を修正します。
distanceSensor.xml
#highlight(xml:firstline[40]){{
<set-attr-value name="implementation" value="./distanceS...
}}
↓
#highlight(xml:firstline[40]){{
<set-attr-value name="implementation" value="./distanceS...
}}
視線ベクトルを修正します。(※下を向いていると地面を検出し...
#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
ロボットがリビングに立っているので好きなところでメッセー...
#ref(distance_2.jpg)
次に保存した深度画像を見てみましょう。
以下のような濃淡画像が見えると思います。
#ref(view001.jpg)
深度画像の例
#highlight(end)
遠いオブジェクトほど色が薄くなり、近いオブジェクトほど色...
Up:[[Tutorial]] Previous:[[車輪移動ロボット]] ...
Page: