![[PukiWiki]](image/Concept_logo.jpg "[PukiWiki]"){kind=logo}
Up:[[Tutorial]] Previous：[[Wheeled mobile robot]] Next: [[眼球運動]]
#contents
*Distance sensor [#m5e8543a]
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 [#r77819d7]
This sample uses a function distanceSensor() to get a distance between the camera and a target object which is gazed by the camera.
*** Controller [#h7f70978]
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:
//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 );
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.
#highlight(cpp:firstline[62]){{
distance = m_view->distanceSensor();
}}
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:
change
#highlight(cpp:firstline[62]){{
distance = m_view->distanceSensor();
}}
to
#highlight(cpp:firstline[62]){{
distance = m_view->distanceSensor(50.0, 500.0, 2);
}}
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.
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.
In this controller, a robot keeps on going forward; if the distance is shorter than 100cm, the direction will be changed randomly.
Compile the source code of the controller.
$ ./sigmake.sh distanceSensor.cpp
*** World File [#w89a6566]
Next, edit a world file
$ 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">
<!--Name of agent-->
<set-attr-value name="name" value="robot_000"/>
<set-attr-value name="language" value="c++"/>
<!--Setting of controller-->
<set-attr-value name="implementation" value="./distanceSensor.so"/>
<!--Do no use dynamics simulation-->
<set-attr-value name="dynamics" value="false"/>
<!--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"/>
<!--Setting of camera parameters-->
<camera id="1"
link="HEAD_LINK"
direction="0 0 1"
position="0.0 0.0 5.0"
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="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="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="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="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="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.
*** Execution [#mf620fe8]
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.
** Measurement of distance vector [#l8f8546a]
Next, an example that measure distance vector along a horizontal plane.
*** 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;
}
}}
In this example, the robot changes direction when the shortest distance in the distance vector is shorter than 100cm.
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.
*** Seeting of view angle [#le004fb7]
You can change the horizontal view angle with vertical view angle and aspect ratio in a world file.
Due to the resolution quality, the recommended horizontal angle is from 30(deg) to 80(deg).
*** 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(xml:firstline[13]){{
<!--assignment of controller-->
<set-attr-value name="implementation"
value="./distanceSensor1D.so"/>
}}
*** Execution [#m3c8b021]
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));
//距離が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() 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]
$ emacs distanceSensor.xml
Change the controller from distanceSensor1D.so to distanceSensor2D.so.
*** 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.bmp)
#highlight(end)
Further distance is shown by dark; shorter distance is shown by light.
Up:[[Tutorial]] Previous：[[Wheeled mobile robot]] Next: [[眼球運動]]
Site admin: inamura
PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.