概要
ここではサンプルとして付属しているSamplePD(人間型ロボットが歩行を行うサンプル）を拡張して、ビューシミュレーション機能を利用する手順を説明します。主な手順は以下のようになります。
- カメラモデルの設定
- シミュレーションプロジェクトの設定変更
- コントローラの変更
- コントローラブリッジの設定変更
以下これらの手順について詳細に説明します。
カメラモデルの設定
まず最初にロボットあるいは環境オブジェクトに取り付けられているカメラの情報を対応するVRMLファイルに記述します。カメラ情報の記述はVRMLモデルにVisionSensorノードを追加することによって行います。
ここではロボット頭部に2台のカメラが取り付けられていることとします。SamplePDのシミュレーションで用いられているロボットのモデルはsample/model/sample.wrlに収録されています。このファイルを開くと頭部のリンク(CHEST)に以下のように既に2台のカメラが取り付けられています。
:
:
DEF CHEST Joint {
:
:
children [
:
:
DEF VISION_SENSOR1 VisionSensor {
translation 0.15 0.05 0.15
rotation 0.4472 -0.4472 -0.7746 1.8235
name "LeftCamera"
type "DEPTH"
sensorId 0
children [
DEF CAMERA_SHAPE Transform {
rotation 1 0 0 -1.5708
children [
Shape {
geometry Cylinder {
radius 0.02
height 0.018
}
appearance Appearance {
material Material {
diffuseColor 1 0 0
}
}
}
]
}
]
}
DEF VISION_SENSOR2 VisionSensor {
translation 0.15 -0.05 0.15
rotation 0.4472 -0.4472 -0.7746 1.8235
name "RightCamera"
type "DEPTH"
sensorId 1
children [
USE CAMERA_SHAPE
]
}
:
:
2台のカメラとも、出力する画像のタイプが"DEPTH"（距離画像） となっています。ここでは画像タイプを次のように"COLOR"に変更します。
DEF VISION_SENSOR1 VisionSensor {
:
:
type "COLOR"
:
:
}
DEF VISION_SENSOR2 VisionSensor {
:
:
type "COLOR"
:
:
}
VisionSensorノードの属性値を変更することで画像のピクセル数や画角などを変更することができます。詳しくはこちらを参照して下さい。
シミュレーションプロジェクトの変更
モデルにカメラが取り付けられていてもコントローラが画像データを必要としない場合も考えられるため、ビューシミュレーション機能はデフォルトでOFFになっています。これを有効にするにはGrxUIを用いてシミュレーションプロジェクトの設定を変更してビューシミュレーション機能をONにする必要があります。
OpenHRP Viewのsimulationタブを開き、ViewSimulationチェックボックスにチェックを入れて下さい。これでビューシミュレーション機能が有効になります。
この段階で一度シミュレーションを実行してみて下さい。ビューシミュレーション機能が有効になっている状態でシミュレーションを開始すると、開始と同時にカメラからの画像を表示するウィンドウが現れ、シミュレーションの進行と共に表示される画像も更新されます。今回の場合2台のカメラが取り付けられているのでウィンドウが2つ表示されます。
コントローラの変更
画像を利用した処理をコントローラに追加するため、コントローラのソースファイルsample/controller/SamplePD/SamplePD.{h, cpp}を編集します。
ここでは非常に単純な処理ですが画像の中心から左右それぞれ10ピクセル分の情報を表示することにします。
まず最初に画像を入力するためのポートを追加します。カラー画像はRTC::TimedLongSeq型のデータとしてコントローラブリッジから出力されますので、このデータ型で"image"という名前を持つ入力ポートを一つ追加します。以下の赤字の部分を追加して下さい。
SamplePD.h
:
:
// DataInPort declaration
// <rtc-template block="inport_declare">
TimedDoubleSeq m_angle;
InPort<TimedDoubleSeq> m_angleIn;
TimedLongSeq m_image;
InPort<TimedLongSeq> m_imageIn;
// </rtc-template>
:
:
SamplePD.cpp
:
:
SamplePD::SamplePD(RTC::Manager* manager)
: RTC::DataFlowComponentBase(manager),
// <rtc-template block="initializer">
m_angleIn("angle", m_angle),
m_imageIn("image", m_image),
m_torqueOut("torque", m_torque),
:
:
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
registerInPort("angle", m_angleIn);
registerInPort("image", m_imageIn);
:
:
次に実際に画像を入力ポートから読み込んで処理（今回の場合はピクセルの情報を表示）する部分を追加します。こちらも赤字の部分です。
SamplePD.cpp
:
:
RTC::ReturnCode_t SamplePD::onExecute(RTC::UniqueId ec_id)
{
:
:
if(m_angleIn.isNew()){
m_angleIn.read();
}
if (m_imageIn.isNew()){
m_imageIn.read();
#if 0
static ::CORBA::ULong startl = 0U;
if(startl == 0)
startl = m_image.tm.sec;
printf("t = %4.1f[s] : ",m_image.tm.sec - startl + m_image.tm.nsec*1e-9);
#else
static int count = 0;
printf("t = %4.1f[s] : ", count*0.1);
count++;
#endif
#define isBlack(x) (x)==0xff000000
for (int i=320*120+150; i<320*120+170; i++){
if (isBlack(m_image.data[i])){
printf("0");
}else{
printf("1");
}
}
printf("\n");
}
:
:
今回の場合、入力される画像サイズは320x240です。これはVisionSensorノードのwidth, height属性値で変更することができます。各ピクセルの色情報は1ピクセル当たり4バイトの情報としてTimedLongSeqのdata部分に格納されています。この例では画像データに含まれる時刻の情報と、各ピクセルの色が黒であれば'0'、それ以外であれば'1'を表示するという処理を行っています。
コントローラブリッジの設定変更
次にコントローラブリッジから画像情報を出力させるための設定変更を行います。コントローラブリッジの設定は、Linuxの場合 sample/controller/SamplePD/SamplePD.sh, Windowsの場合 sample/controller/SamplePD/SamplePD.bat で行っていますので、これを以下のように変更します。赤字部分が変更部分です。
#!/bin/sh
openhrp-controller-bridge \
--server-name SamplePDController \
--out-port angle:JOINT_VALUE \
--out-port left-eye:0:COLOR_IMAGE:0.1 \
--in-port torque:JOINT_TORQUE \
--connection angle:angle \
--connection torque:torque \
--connection left-eye:image
最初の変更部分がコントローラブリッジに画像出力を行うポートを追加している部分です。ここではロボットに搭載されている2台のカメラのうち左側のカメラの画像を出力することにします。左側のカメラのsensorIdは0ですので識別子として0を指定します。また出力ポートの名前を"left-eye"、出力周期を0.1[s]毎と指定しています。
最後の変更部分はコントローラブリッジとSamplePDコンポーネントの接続を指定する部分です。先ほどSamplePDコンポーネントに追加したimageポートとコントローラブリッジに追加したleft-eyeポートを接続するように指定しています。
コントローラブリッジの設定の詳細に関しては
こちらを参照して下さい。
シミュレーションの実行
以上で全ての設定変更は完了しましたので、シミュレーションを実行して下さい。コントローラからの以下のような出力がProcessManager Viewに表示されます。
setting naming
setup RT components
detected SamplePD0
detected the ExtTrigExecutionContext of SamplePD0
connect VirtualRobot0:angle <--> SamplePD0:angle ...ok
connect VirtualRobot0:torque <--> SamplePD0:torque ...ok
connect VirtualRobot0:left-eye <--> SamplePD0:image ...ok
on Activated
t = 0.0[s] : 00000000000000000001
t = 0.1[s] : 00000000000000001000
t = 0.2[s] : 00000000000000000100
t = 0.3[s] : 00000000000000001000
t = 0.4[s] : 00000000000000001000
t = 0.5[s] : 00000000000000010000
t = 0.6[s] : 00000000000000010000
t = 0.7[s] : 00000000000000100000
t = 0.8[s] : 00000000000001000000
t = 0.9[s] : 00000000000010000000
t = 1.0[s] : 00000000000100000000
t = 1.1[s] : 00000000001000000000
t = 1.2[s] : 00000000001000000000
t = 1.3[s] : 00000000010000000000
t = 1.4[s] : 00000000010000000000
t = 1.5[s] : 00000000010000000000
t = 1.6[s] : 00000000010000000000
t = 1.7[s] : 00000000010000000000
:
:
:
3DViewで地面のグリッド表示をONにしている場合、生成される視野画像にもグリッドが含まれますので、視野内のグリッドがロボットの揺動によって移動するため上記のような出力が得られます。
