Real-Time Linuxによるリアルタイム処理とOpenGL によるシミュレータの構築 (4)

index	section 1	section 2	section 3	section 4	section 5	section 6
bibrography	appendix A	appendix B	appendix C

4. リアルタイム処理と3次元アニメーション

本節では、これまでに説明したRTLinuxによるリアルタイム処理と OpenGLによる3次元アニメーションを融合させることについて説明する。 3次元アニメーションは一般に非常に重い処理であるため、アニメーションを含むアプリケーション内で周期的な処理を行おうとすると、次の実行周期までに画面の描画が終了できずに周期が守れくなるという危険性がある。しかし、アニメーションと周期的な処理を切り放し、周期的な処理にCPUの優先的な使用権を与えれば、万が一画面の描画が終了しなかった場合でも周期的な処理は実行できる。

4.1 基本設計方針

RTLinux のサンプルの timer_module.c にはほとんど手を加えない。
OpenGL のモデリングは別のファイルで定義する。
通信の部分をOpenGL用に書いたソースコードに埋め込んで行く。

RT-module と OpenGLモデルはそれぞれ別々に作成しておいた方が、部品としての独立性が高まり、単体での試験が可能となる上、移植性も良くなる。 OpenGLのプログラムに比べれば、RTLinuxプログラムのメイン関数は通信のみの機能しか持たず単純であるから、OpenGL用に書いたプログラムのソースコードに RTLinux用の通信の部分を挿入すれば楽にプログラムを作成できる。

4.2 リアルタイム処理と3次元グラフィクスを同時に扱うサンプルプログラム

RTLinuxによるリアルタイム処理と OpenGLによる3次元グラフィクスを同時に扱うプログラムの例として、ストップウォッチ・プログラムを作成した。サンプルプログラムの一部を用いて、どのようにして同時に取り扱うのかを説明する。アプリケーション側の流れは、以下のようになる。

glutおよびウィンドウの初期化、生成
RT-FIFOを開く
イベントハンドラ（コールバック関数）の登録
イベント処理の開始
- マウスイベント
  - スレッドの起動／停止
  - スレッド停止、プログラムの終了
- レンダリングイベント
- アイドル(通信待ち)

OpenGLプログラムの処理にRTLinuxの通信が加わったところだけを説明する。まずはじめに、アプリケーションが始まるとウィンドウの初期化、生成を行うので、ここに FIFOオープンも一緒に行う。


void init(void)
{
  （ 中 略 ）
  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
  glEnable(GL_DEPTH_TEST);

  // 視点の初期設定
  Camera.pos[2] = 7.0;
  orgCamera = Camera;

  // Open RT-FIFO
  if ((ctl = open("/dev/rtf1", O_WRONLY)) < 0) {
    fprintf(stderr, "Error opening /dev/rtf1\n");
    exit(1);
  }  
  if ((fd0 = open("/dev/rtf3", O_RDONLY)) < 0) {
    fprintf(stderr, "Error opening /dev/rtf3\n");
    exit(1);
  }

}

ウィンドウの初期化とFIFOオープンはどちらが先でも良いが、 initという関数があったので、ここではそこで行うことにした。次に、スレッドの起動／停止メッセージは、マウス操作によりアニメーションループの起動／停止を行うところに組み込む。


void mouse(int button, int state, int x, int y)
{

  switch ( button ) {
    
  case GLUT_LEFT_BUTTON :
    if(state == GLUT_DOWN){

      if( START_THREAD != Msg.command ){
        // start the thread  
        Msg.command = START_THREAD;
        Msg.period = dt;
        if (write(ctl, &Msg, sizeof(STMsg)) < 0) {
          fprintf(stderr, "Can't send a command to RT-thread\n");
          exit(1);
        }
        
      }
      else{
        // stop the thread
        Msg.command = STOP_THREAD;
        if (write(ctl, &Msg, sizeof(STMsg)) < 0) {
          fprintf(stderr, "Can't send a command to RT-thread\n");
          exit(1);
        }
        
      }
    } 
    break;
    
  case GLUT_MIDDLE_BUTTON :
    if(state == GLUT_DOWN){
      // reset and stop the thread
      Msg.command = RESET_THREAD;
      if (write(ctl, &Msg, sizeof(STMsg)) < 0) {
        fprintf(stderr, "Can't send a command to RT-thread\n");
        exit(1);
      }
      
    }
    break;
    
  case GLUT_RIGHT_BUTTON :
    if(state == GLUT_DOWN){
      // stop the thread
      Msg.command = STOP_THREAD;
      if (write(ctl, &Msg, sizeof(STMsg)) < 0) {
        fprintf(stderr, "Can't send a command to RT-thread\n");
        exit(1);
      }
      exit(0);
    }  
  
  default :
    break;
    
  }
  
}

マウスの左ボタンを押すと、スレッドに起動メッセージが送られる。それと同時に、アプリケーション側は手の空いている限り関数 timeUpdate を実行する。この関数は、スレッドからデータを受け取る関数で、以下のようになっている。


void timeUpdate(void)
{
  fd_set         rfds;
  struct timeval tv;
  int            retval;

  FD_ZERO(&rfds);
  FD_SET(fd0, &rfds);
  tv.tv_sec =  dt*3; 
  tv.tv_usec = 0;
  
  retval = select(FD_SETSIZE, &rfds, NULL, NULL, &tv);
  if (retval > 0) {
    if (FD_ISSET(fd0, &rfds)) {
      read(fd0, &Data, sizeof(STData) );
      printf("time : %4.2f  \n", Data.sec );
    }
  }
  
  glutPostRedisplay();
}

この関数によりスレッドからデータを受け取り、画面を再描画するよう指示がだされる。 timer_module 側には、リセット命令として以下のような記述を追加した。


    if( err == sizeof(STMsg) ){
      
      switch (Msg.command) {
      case START_THREAD :
        pthread_make_periodic_np(pthread_self(), 
                                 gethrtime(), 
                                 real2in(Msg.period) );
        break;

      case STOP_THREAD :
        pthread_suspend_np(pthread_self());
        break;

      case RESET_THREAD :
        Data.sec = 0.0;
        rtf_put(DATA_FIFO, (char *)(&Data), sizeof(STData) );
        pthread_suspend_np(pthread_self());
        break;

      default:
        rtl_printf("RTL task: bad command\n");
        return 0;
      }
    }

以上が、RTLinux によるリアルタイム処理とOpenGLによる3次元グラフィクスを同時に扱うプログラムの書き方である。プログラムを書く上で最後に注意しておいてほしいことは、プログラムを終了させる前に、確実にスレッドを停止することである。スレッドが起動したままの状態でプログラムが終了してしまうと、 rmmod でモジュールを取り除くことができなくなるからである。

4.3 ロボット制御への応用

リアルタイム処理と3次元グラフィクスを組み合わせた応用例として 4脚ロボットの制御とそのシミュレータを紹介する。

このプログラムでは、制御計算は RT-module で行い、ユーザインターフェイスやシミュレータは OpenGLで表示しているため、ロボットのリアルタイム制御を行いながら、コンピュータの画面に3次元アニメーションでロボットの姿勢をリアルタイムで表示することが可能となっている。ロボットの制御の流れを下図に示す。　スレッド内で行われる制御計算は、歩容計画から脚先端軌道計画、逆行列の計算を含む軌道追従制御まで負荷の大きい計算をしているが、スレッドの実行周期を速くしてもアニメーションが飛んでしまうだけで、制御計算は確実に行われている。

図　ロボット制御の流れ
クリックすると拡大図が出ます

next >>