自作の RTC で制御
初心者用課題
ひな形コードの作成
新規作成した RTC を EducatorVehicle と接続して制御するまでの手順を説明します。
まずは Windows、Ubuntu上で このページ の手順に従って RTC を作成してください。 RTC の仕様は以下のように入力します。
| 基本 | ||
| モジュール名 | EV3SampleCPP、もしくはEV3SamplePy | |
| アクティビティ | ||
| 有効アクション | onInitialize、onExecute、onActivated、onDeactivated | |
| データポート | ||
| InPort | ||
| 名前 | データ型 | 説明 |
| touch | RTC::TimedBooleanSeq | タッチセンサーのオンオフ |
| OutPort | ||
| 名前 | データ型 | 説明 |
| target_velocity | RTC::TimedVelocity2D | 目標速度 |
| sound | RTC::TimedString | 音声 |
| コンフィギュレーション | ||
| 名前 | 型 | 説明 |
| forward_velocity | double | 直進速度、デフォルト値は0.0、制約条件に-0.20<=x<=0.20、Widgetはslider、Stepは0.02 |
| rotate_velocity | double | 回転速度、デフォルト値は0.0、制約条件に-3.1<=x<=3.1、Widgetはslider、Stepは0.2 |
| 言語・環境 | ||
| 言語 | C++、もしくは Python | |
この RTC ではコンフィギュレーションパラメーター forward_velocity、rotate_velocity に入力した速度を target_velocity から EducatorVehicle に送信します。 さらに touch でタッチセンサーのオンオフを取得して、オンになった場合は停止して sound から発声する音を指令します。
※RTC は EV3上で動作させることを前提にしていますが、動作確認や講習会での利用には Windows や Ubuntu上で動作させても問題はないので、その場合はコードの編集をする前に CMake で Visual Studio、もしくは Code::Blocks のプロジェクトを生成しておくことをお勧めします。 CMake でプロジェクト生成からビルドまでの手順は以下のページに記載してあります。
コードの編集
まずは onExecute 関数を編集します。
以下はforward_velocity、rotate_velocityの 値を target_velocity から送信するコードです。
C++(src/EV3SampleCPP.cpp)
RTC::ReturnCode_t EV3SampleCPP::onExecute(RTC::UniqueId ec_id)
{
//コンフィギュレーションパラメータで設定した速度を送信する
m_target_velocity.data.vx = m_forward_velocity;
m_target_velocity.data.va = m_rotate_velocity;
setTimestamp(m_target_velocity);
m_target_velocityOut.write();
return RTC::RTC_OK;
}Python(EV3SamplePy.py)
def onExecute(self, ec_id):
#コンフィギュレーションパラメータで設定した速度を送信する
self._d_target_velocity.data.vx = self._forward_velocity[0]
self._d_target_velocity.data.vy = 0
self._d_target_velocity.data.va = self._rotate_velocity[0]
OpenRTM_aist.setTimestamp(self._d_target_velocity)
self._target_velocityOut.write()一応、非アクティブ状態の時は停止するように onDeactivated で速度0を送信するようにします。
C++(src/EV3SampleCPP.cpp)
RTC::ReturnCode_t EV3SampleCPP::onDeactivated(RTC::UniqueId ec_id)
{
//停止する
m_target_velocity.data.vx = 0;
m_target_velocity.data.vy = 0;
m_target_velocity.data.va = 0;
setTimestamp(m_target_velocity);
m_target_velocityOut.write();
return RTC::RTC_OK;
}Python(EV3SamplePy.py)
def onDeactivated(self, ec_id):
#停止する
self._d_target_velocity.data.vx = 0
self._d_target_velocity.data.vy = 0
self._d_target_velocity.data.va = 0
OpenRTM_aist.setTimestamp(self._d_target_velocity)
self._target_velocityOut.write()
return RTC.RTC_OK共通インターフェース仕様書では進行方向をX軸正方向にしているため、Velocity2D型のvxに直進速度、vaに回転速度を入力します。
Pythonではさらにコンストラクタの以下の部分を修正してください。
Python(EV3SamplePy.py)
#self._d_target_velocity = RTC.TimedVelocity2D(*target_velocity_arg)
self._d_target_velocity = RTC.TimedVelocity2D(RTC.Time(0,0),RTC.Velocity2D(0,0,0)) #self._d_sound = RTC.TimedString(*sound_arg)
self._d_sound = RTC.TimedString(RTC.Time(0,0),[])※ここまでの作業だけでも Educator Vehicle の動作は可能なので、面倒ならば以下の手順は飛ばしてもらっても大丈夫です。
次にタッチセンサーが ON になった時に停止する処理を書きます。 ここで touch に onExecute が呼び出された時に新規に受信したデータが必ず存在するとは限らないので、タッチセンサーのデータを一時的に格納しておく変数を宣言しておきます。
C++(include/EV3SampleCPP/EV3SampleCPP.h)
private:
bool m_last_sensor_data[2]; /*センサーのデータを格納する変数*/Python(EV3SamplePy.py)
def __init__(self, manager):
OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)
#タッチセンサーのデータを格納する変数
self._last_sensor_data = [False, False]次に onExecute にタッチセンサーがオンの場合に停止、音声を出力する処理を追加します。
C++(src/EV3SampleCPP.cpp)
RTC::ReturnCode_t EV3SampleCPP::onExecute(RTC::UniqueId ec_id)
{
//ここから
//データを新規に受信した場合に、データを m_last_sensor_data を格納する
if (m_touchIn.isNew())
{
m_touchIn.read();
if (m_touch.data.length() == 2)
{
for (int i = 0; i < 2; i++)
{
//タッチセンサーがOFFからONになった時に音を鳴らす
if (!m_last_sensor_data[i] && m_touch.data[i])
{
m_sound.data = "beep";
setTimestamp(m_sound);
m_soundOut.write();
}
m_last_sensor_data[i] = m_touch.data[i];
}
}
}
//タッチセンサーがONの時に前進しないようにする
if (m_forward_velocity > 0)
{
for (int i = 0; i < 2; i++)
{
if (m_last_sensor_data[i])
{
m_target_velocity.data.vx = 0;
m_target_velocity.data.vy = 0;
m_target_velocity.data.va = 0;
setTimestamp(m_target_velocity);
m_target_velocityOut.write();
return RTC::RTC_OK;
}
}
}
//ここまで
//コンフィギュレーションパラメーターで設定した速度を送信する
m_target_velocity.data.vx = m_forward_velocity;
(以下略)Python(EV3SamplePy.py)
def onExecute(self, ec_id):
#ここから
#データを新規に受信した場合に、データをm_last_sensor_dataを格納する
if self._touchIn.isNew():
data = self._touchIn.read()
if len(data.data) == 2:
for i in range(2):
#タッチセンサーがOFFからONになった時に音を鳴らす
if not self._last_sensor_data[i] and data.data[i]:
self._d_sound.data = "beep"
OpenRTM_aist.setTimestamp(self._d_sound)
self._soundOut.write()
self._last_sensor_data = data.data[:]
#タッチセンサーが ON の時に前進しないようにする
if self._forward_velocity[0] > 0:
for d in self._last_sensor_data:
if d:
self._d_target_velocity.data.vx = 0
self._d_target_velocity.data.vy = 0
self._d_target_velocity.data.va = 0
OpenRTM_aist.setTimestamp(self._d_target_velocity)
self._target_velocityOut.write()
return RTC.RTC_OK
#ここまで
#コンフィギュレーションパラメーターで設定した速度を送信する
self._d_target_velocity.data.vx = self._forward_velocity[0]
(以下略)ビルド
ソースコードの編集が終了したらC++の場合はビルドを行います。 EV3上で動作させる場合はクロスコンパイル環境でビルドを行ってください。 ソースコードのあるディレクトリに移動して以下のコマンドを入力すればビルドできます。
cd EV3SampleCPP cmake . make
※Visual Studio、もしくは Code::Blocks で編集している場合は GUI上の操作でビルドを行ってください。
生成された src/EV3SampleCPPComp を EV3 に転送してください。 ※Windows、Ubuntu で動作させる場合は転送しないでください。
sftp robot@<IPアドレス> sftp> put EV3SampleCPP/src/EV3SampleCPPComp
Python版もEV3SamplePy.py を転送してください。
sftp robot@<IPアドレス> sftp> put EV3SamplePy/EV3SamplePy.py
cmake がインストールされていない場合は以下のコマンドを入力してください。
sudo apt-get install cmake
RTC 起動
ビルドが完了したら RTC を起動します。 以下のコマンドで起動できます。C++
EV3SampleCPPComp&
Python
python EV3SamplePy.py&
※Windows上で RTC を起動する場合は EV3SampleCPPComp.exe (EV3SamplePy.py) をダブルクリックしてください。
次に EducatorVehicle を起動させます。 以下のコマンドで起動してください。
Compomnents/EducatorVehicleComp&
動作確認
まずは動作確認のためにデータポートの接続を行います。 Windows側で RTシステムエディタを起動してください。 最初に EV3 で起動しているネームサーバーを RTシステムエディタのビューに追加します。 ネームサーバー追加ボタンを押下して EV3 の IPアドレスを入力してください。
ネームサーバーを追加したらデータポートを以下のように接続してください。
| ポートの接続 | |
| EV3SampleCPP0(EV3SamplePy0) | EducatorVehicle0 |
| target_velocity | velocity2D |
| touch | touch |
| sound | sound |
そして RTC をアクティブ化すると動作を開始します。
まずは EV3SampleCPP(EV3SamplePy) のコンフィギュレーションパラメーターを変更する事で Educator Vehicle を操作してみます。
RTシステムエディタで EV3SampleCPP0(EV3SamplePy0) を選択して、Configuration View の編集を選択してください。
すると以下のウインドウが起動します。
forward_velocity と rotate_velocity のスライダーによりEV3を操作できます。
次にタッチセンサーがオンになった場合に停止するかを確認してみます。 タッチセンサーに物体を接触させて停止するかを確認してください。 停止した時に音が鳴るかも確認してください。
応用(中級者用課題)
ここからはさらにタッチセンサーがオンになった場合に後退→回転のより障害物を回避する動作を実装します。 タッチセンサーのデータが入力された時に以下の処理を実行することで回避運動を行います。
コンフィギュレーションパラメーター
新たに以下のコンフィギュレーションパラメーターを設定します。
| コンフィギュレーション | ||
| 名前 | 型 | 説明 |
| back_speed | double | 後退速度、デフォルト値は0.1 |
| back_time | double | 後退運動の時間、デフォルト値は1.0 |
| rotate_speed | double | 回転速度、デフォルト値は0.8 |
| rotate_time | double | 回転運動の時間、デフォルト値は2.0 |
関数の実装
新たに以下の関数を実装します。
| 関数名 | 内容 | 引数 |
| stop_robot | 停止する | なし |
| back_move | 後退する | なし |
| rotate_move | 回転する | dir(回転方向) |
stop_robot 関数
stop_robot関数は以下のようになっています。 目標速度を0に設定して送信します。
void ControlEducatorVehicle::stop_robot()
{
m_target_velocity_out.data.vx = 0;
m_target_velocity_out.data.vy = 0;
m_target_velocity_out.data.va = 0;
setTimestamp(m_target_velocity_out);
m_target_velocity_outOut.write();
}back_move 関数
back_move 関数は以下のようになっています。 後退するための速度を出力後、sleep 関数で指定時間だけ待つことによりコンフィギュレーションパラメータ back_speed の 速度でback_time の時間だけ後退する運動を実現しています。
void ControlEducatorVehicle::back_move()
{
//後退運動を指令
m_target_velocity_out.data.vx = -m_back_speed;
m_target_velocity_out.data.vy = 0;
m_target_velocity_out.data.va = 0;
setTimestamp(m_target_velocity_out);
m_target_velocity_outOut.write();
//一定時間待つ
double sec, usec;
usec = modf(m_back_time, &sec);
coil::TimeValue ts((int)sec, (int)(usec*1000000.0));
coil::sleep(ts);
//一旦停止
stop_robot();
}※onExecute 関数内で sleep 関数などで待機すると、その間は RTシステムエディタ等による操作ができなくなるためあまり望ましい実装の方法ではありません。 ただ今回の場合は実行コンテキストを止めずに処理を実装すると処理が複雑化するためこのように実装しています。
stop_robot 関数
rotate_move 関数は以下のようになっています。 後退運動と同じく、コンフィギュレーションパラメーター rotate_speed の速度で rotate_time の時間だけ回転運動する処理を実装しています。
void ControlEducatorVehicle::rotate_move(bool dir)
{
//回転運動を指令
m_target_velocity_out.data.vx = 0;
m_target_velocity_out.data.vy = 0;
//左回転
if (dir)m_target_velocity_out.data.va = m_rotate_speed;
//右回転
else m_target_velocity_out.data.va = -m_rotate_speed;
setTimestamp(m_target_velocity_out);
m_target_velocity_outOut.write();
//一定時間待つ
double sec, usec;
usec = modf(m_rotate_time, &sec);
coil::TimeValue ts((int)sec, (int)(usec*1000000.0));
coil::sleep(ts);
//一旦停止
stop_robot();
}onExecute 関数の実装
最後にフローチャートのように、タッチセンサーのデータ入力時に回避運動を行う処理を実装すれば完成です。
RTC::ReturnCode_t ControlEducatorVehicle::onExecute(RTC::UniqueId ec_id)
{
//目標速度を変数に格納
if (m_target_velocity_inIn.isNew())
{
m_target_velocity_inIn.read();
vx = m_target_velocity_in.data.vx;
vy = m_target_velocity_in.data.vy;
va = m_target_velocity_in.data.va;
}
//タッチセンサーの処理
if (m_touchIn.isNew())
{
while(m_touchIn.isNew())m_touchIn.read();
if (m_touch.data.length() >= 2)
{
touch_r = m_touch.data[0];
touch_l = m_touch.data[1];
//前進しているときのみ
if (vx > 0)
{
//左タッチセンサー
if (m_touch.data[0])
{
停止→後退→右回転
stop_robot();
back_move();
rotate_move(true);
}
//右タッチセンサー
else if (m_touch.data[1])
{
停止→後退→左回転
stop_robot();
back_move();
rotate_move(false);
}
}
}
}
m_target_velocity_out.data.vx = vx;
m_target_velocity_out.data.vy = vy;
m_target_velocity_out.data.va = va;
setTimestamp(m_target_velocity_out);
//目標速度送信
m_target_velocity_outOut.write();
return RTC::RTC_OK;
}発展(上級者用課題)
ここでは超音波センサーにより地面までの距離を検知して滑落を回避する RTC を作成します。
超音波センサーで検知した地面までの高さが一定以上の場合に、以下の処理を実行します。
データポート、コンフィギュレーションパラメーター
以下のデータポート、コンフィギュレーションパラメーターを追加してください。
| データポート | ||
| InPort | ||
| 名前 | データ型 | 説明 |
| ultrasonic | RTC::RangeData | 超音波センサーをレンジセンサーと仮定し、要素1の距離データを格納 |
| current_pose | RTC::TimedPose2D | 現在の位置・姿勢 |
| OutPort | ||
| 名前 | データ型 | 説明 |
| angle | RTC::TimedDouble | モーターMの角度 |
| コンフィギュレーション | ||
| 名前 | 型 | 説明 |
| sensor_height | double | 接地可能と判定する地面までの高さ、デフォルト値は0.20 |
| medium_motor_range | double | Mモーターの動作範囲、デフォルト値は1.6 |
関数の実装
接地可能な地面を超音波センサーで調べる search_ground 関数と、ロボットを指定した角度だけ回転させる turn_move 関数を実装します。
bool ControlEducatorVehicle::search_ground(double &a)
{
//Mモーターを右回転
m_angle.data = (-1)*m_medium_motor_range;
setTimestamp(m_angle);
m_angleOut.write();
coil::TimeValue ts(2, 0);
coil::sleep(ts);
//地面までの距離が一定以下かを判定
if (m_ultrasonicIn.isNew())
{
while(m_ultrasonicIn.isNew())m_ultrasonicIn.read();
if (m_ultrasonic.ranges.length() >= 1)
{
if (m_ultrasonic.ranges[0] < m_sensor_height)
{
//一定以下の場合は接地可能と判定して、接地可能な地面を検知した方向を変数に格納
a = m_medium_motor_range;
return true;
}
}
}
//Mモーターを左回転
m_angle.data = (-1)*-m_medium_motor_range;
setTimestamp(m_angle);
m_angleOut.write();
ts = coil::TimeValue(4, 0);
coil::sleep(ts);
//地面までの距離が一定以下かを判定
if (m_ultrasonicIn.isNew())
{
while(m_ultrasonicIn.isNew())m_ultrasonicIn.read();
if (m_ultrasonic.ranges.length() >= 1)
{
if (m_ultrasonic.ranges[0] < m_sensor_height)
{
//一定以下の場合は接地可能と判定して、接地可能な地面を検知した方向を変数に格納
a = -m_medium_motor_range;
return true;
}
}
}
m_angle.data = 0;
setTimestamp(m_angle);
m_angleOut.write();
return false;
} void ControlEducatorVehicle::turn_move(double a)
{
int max_count = 100;
coil::TimeValue ts(0,10000);
//現在の姿勢を取得する
double spos = 0;
for (int i = 0; i < max_count; i++)
{
if (m_current_poseIn.isNew())
{
m_current_poseIn.read();
spos = m_current_pose.data.heading;
break;
}
coil::sleep(ts);
//一定時間内にデータを取得できなかった場合は回転運動を中止する
if (i == max_count - 1)return;
}
max_count = 1000;
//回転運動を開始する
double data_new_count = 0;
double max_data_new_count = 100;
for (int i = 0; i < max_count; i++)
{
if (m_current_poseIn.isNew())
{
while(m_current_poseIn.isNew())m_current_poseIn.read();
//目標姿勢角との差に定数を掛けた値を目標速度とする
double pos = m_current_pose.data.heading - spos;
double k = 1;
m_target_velocity_out.data.vx = 0;
m_target_velocity_out.data.vy = 0;
double diff = (a - pos);
double vela = k * diff;
if(vela > m_rotate_speed)vela = m_rotate_speed;
if(vela < -m_rotate_speed)vela = -m_rotate_speed;
//目標速度出力
m_target_velocity_out.data.va = vela;
setTimestamp(m_target_velocity_out);
m_target_velocity_outOut.write();
//目標姿勢角との差が一定以下の場合は停止して処理終了
if(sqrt(pow(diff,2)) < 0.03)
{
stop_robot();
return;
}
data_new_count = 0;
}
else
{
//一定時間データが取得できなかった場合は処理終了
data_new_count += 1;
if(data_new_count > max_data_new_count)
{
stop_robot();
return;
}
coil::sleep(ts);
}
}
stop_robot();
}onExecute 関数
最後に onExecute 関数を以下のように実装します。
RTC::ReturnCode_t ControlEducatorVehicle::onExecute(RTC::UniqueId ec_id)
{
//目標速度を変数に格納
if (m_target_velocity_inIn.isNew())
{
m_target_velocity_inIn.read();
vx = m_target_velocity_in.data.vx;
vy = m_target_velocity_in.data.vy;
va = m_target_velocity_in.data.va;
}
//超音波センサーのデータ入力時の処理開始
if (m_ultrasonicIn.isNew())
{
while(m_ultrasonicIn.isNew())m_ultrasonicIn.read();
if (m_ultrasonic.ranges.length() >= 1)
{
range = m_ultrasonic.ranges[0];
//前進しているかの判定
if (vx > 0)
{
//超音波センサーで検知した地面までの高さが一定以上かの判定
if (m_ultrasonic.ranges[0] > m_sensor_height)
{
//停止フラグをtrueにする
//(次の超音波センサーデータ入力時に距離が一定以下と判定した場合は停止フラグをfalseにする)
stop_flag = true;
//一旦停止
stop_robot();
//Mモーターの角度を0に設定
m_angle.data = 0;
setTimestamp(m_angle);
m_angleOut.write();
//Mモーターを回転させて接地可能な地面を検知
double a = 0;
bool ret = search_ground(a);
//Mモーターの角度を0に設定
m_angle.data = 0;
setTimestamp(m_angle);
m_angleOut.write();
//接地可能な地面が見つかった場合は回転運動を行う
if (ret)
{
turn_move(a);
}
coil::TimeValue ts(2, 0);
coil::sleep(ts);
}
//超音波センサーで検知した地面までの高さが一定以下の場合は停止フラグを false にする
else
{
stop_flag = false;
}
}
}
}
//停止フラグが true の場合は停止する
if (stop_flag)
{
m_target_velocity_out.data.vx = 0;
m_target_velocity_out.data.vy = 0;
m_target_velocity_out.data.va = 0;
}
//停止フラグがfalseの場合は目標速度をそのまま出力
else
{
m_target_velocity_out.data.vx = vx;
m_target_velocity_out.data.vy = vy;
m_target_velocity_out.data.va = va;
}
setTimestamp(m_target_velocity_out);
m_target_velocity_outOut.write();
return RTC::RTC_OK;
}RTシステムの保存について
RTシステムの保存については RTシステムエディタ上でもできるのですが、rtshell により RTシステムの復元を自動化することができます。 RTシステムエディタでポートの接続、コンフィギュレーションパラメーターを設定後、rtcryo コマンドを実行してください。
rtcryo localhost -o testSystem.rtsys
これで、testSystem.rtsys というファイルにRTシステムの情報が保存されます。
ただし今回の課題のように複数のネームサーバーに登録された RTC を使用する場合、以下のように複数のネームサーバーを指定する必要があります。
rtcryo localhost 192.168.11.1 -o testSystem.rtsys
保存した RTシステムを復元するためには rtresurrect コマンドを使用します。
rtresurrect testSystem.rtsys
RTCをアクティブ化するには rtstart コマンドを使用します。
rtstart testSystem.rtsys
RTCの非アクティブ化はrtstopコマンドを使用します。
rtstop testSystem.rtsys
ポートを切断する場合は rtteardownコマンドを使用します。
rtteardown testSystem.rtsys
注意点として、RTC はデフォルトの設定だとネームサーバーにはホスト名 .host_cxt 以下に登録されます。 ホスト名は環境によって違うので、この手順で RTシステムを保存しても他の環境では再現できないという事になります。 このため、rtc.conf を編集してネームサーバーの Root 直下に登録するようにすると、他の環境でも RTシステムの復元ができるようになります。
naming.formats: %n.rtc
