自作の 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アドレスを入力してください。

rpm11.png

ネームサーバーを追加したらデータポートを以下のように接続してください。

ev3_sample.png

ポートの接続
EV3SampleCPP0(EV3SamplePy0) EducatorVehicle0
target_velocity velocity2D
touch touch
sound sound

そして RTC をアクティブ化すると動作を開始します。

まずは EV3SampleCPP(EV3SamplePy) のコンフィギュレーションパラメーターを変更する事で Educator Vehicle を操作してみます。

RTシステムエディタで EV3SampleCPP0(EV3SamplePy0) を選択して、Configuration View の編集を選択してください。

ev3_conf.png

すると以下のウインドウが起動します。

ev3_conf2.png

forward_velocity と rotate_velocity のスライダーによりEV3を操作できます。

次にタッチセンサーがオンになった場合に停止するかを確認してみます。 タッチセンサーに物体を接触させて停止するかを確認してください。 停止した時に音が鳴るかも確認してください。

応用(中級者用課題)

ここからはさらにタッチセンサーがオンになった場合に後退→回転のより障害物を回避する動作を実装します。 タッチセンサーのデータが入力された時に以下の処理を実行することで回避運動を行います。

flowChart_4.png

コンフィギュレーションパラメーター

新たに以下のコンフィギュレーションパラメーターを設定します。

コンフィギュレーション
名前 説明
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 を作成します。

 /ja/node/6030/

超音波センサーで検知した地面までの高さが一定以上の場合に、以下の処理を実行します。

flowChart2.png

データポート、コンフィギュレーションパラメーター

以下のデータポート、コンフィギュレーションパラメーターを追加してください。

データポート
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

ダウンロード

最新バージョン : 2.0.1-RELESE

統計

Webサイト統計
ユーザ数:2160
プロジェクト統計
RTコンポーネント307
RTミドルウエア35
ツール22
文書・仕様書2

Choreonoid

モーションエディタ/シミュレータ

OpenHRP3

動力学シミュレータ

OpenRTP

統合開発プラットフォーム

産総研RTC集

産総研が提供するRTC集

TORK

東京オープンソースロボティクス協会

DAQ-Middleware

ネットワーク分散環境でデータ収集用ソフトウェアを容易に構築するためのソフトウェア・フレームワーク