ジョイスティックによるビーゴの遠隔操作 (最後の章だけ執筆中)

ビーゴを用いたジョイスティック操作の遠隔操作デモプログラム

移動中のロボットが、環境からどのような情報を取得しているのかを確認するためのプログラムを作る。移動中のロボットが取得可能な情報を知ることは、自律的な経路生成プログラムを作るためのよい経験になる。よって自律的な経路生成プログラム作成の前段階として、人がロボットを見ながら直接操作、人が遠隔で操作、の２ステップを設ける。

SETP 1. 人がロボットを見ながら操作

STEP 2. 人が遠隔からの環境情報を見ながら操作

STEP 3. ロボット上のソフトウェアが操作

以下、各ステップ毎にプログラムの作り方について述べる。

人がロボットを見ながら操作

ジョイスティック、及びキー入力をロボットへの走行指示に変換する方法について説明する。

ロボットの操作は、並進速度と回転速度を独立に指示するものとする。まず、操作方法は以下の通りとする。

キー入力の場合
- a ... 左回り
- s ... 右回り
- shift ... 前進
- return ... 加速モード / 通常モードのトグル

ジョイスティックの場合
- 左 ... 左回り
- 右 ... 右回り
- 1 ... 前進
- 3 ... 更新
- 6,7 ... 押下時に加速モード

移動ロボットは、並進速度と回転速度を融合した速度で走行するとする。

並進速度、回転速度の融合

並進速度と回転速度が与えられたとき、車輪と速度の関係は以下のようになる。

並進速度: $V_{straight}$
回転速度: $V_{rotate}$
トレッド: $ T $
右輪速度: $V_{right}$
左輪速度: $V_{left}$

$\frac{V_{right} + V_{left}}{2} = V_{straight}$

$\frac{V_{right} - V_{left}}{T} = V_{rotate}$

従って、車輪毎の速度は、

$V_{right} = V_{straight} + \frac{T}{2} * V_{rotate}$

$V_{left} = V_{straight} - \frac{T}{2} * V_{rotate}$

となる。よって、キー、及びジョイスティックによって渡される速度情報を用い、ロボットの車輪毎に目標速度を与えてやればよい。入力情報の処理プログラムは以下のようになる。

static double setVelocity(double now, double ref,
                          double RefAcc, long diff) {
  double sign = ref - now;
  double add = RefAcc * diff / 1000.0;
  double value = now + ((sign > 0.0) ? +add : -add);
  if (sign > 0.0) {
    if (value > ref) {
      value = ref;
    }
  } else {
    if (value < ref) {
      value = ref;
    }
  }
  return value;
}


// 入力の取得
robotInput_t inputHandle(const JoystickCtrl& js,
                         const UserInput::userInput_t& key) {
  static unsigned long pre_ticks = VXV::GetTicks();
  static double now_straight = 0;
  static double now_rotate = 0.0;
  const double RotateRef = (2.0 * M_PI) / 4.0;
  const double RotateAcc = (2.0 * M_PI) * 3.0 / 4.0;
  const int StraightRef = 300;
  const int StraightAcc = 300;

  unsigned long ticks = VXV::GetTicks();
  double ref_straight = 0;
  double ref_rotate = 0.0;
  robotInput_t ri;
  ri.jet = false;

  // キー入力
  if (key.nowPressedCode(SDLK_RSHIFT) || key.nowPressedCode(SDLK_LSHIFT)) {
    ref_straight = StraightRef;
  }
  if (key.nowPressedCode(SDLK_a)) {
    ref_rotate = RotateRef;
  }
  if (key.nowPressedCode(SDLK_s)) {
    ref_rotate -= RotateRef;
  }
  if (key.nowPressedCode(SDLK_RETURN)) {
    ri.jet = true;
  }

  // ジョイスティック入力
  if (js.isActivated()) {
    int js_x = -js.getAxisValue(0);
    ref_rotate += (js_x > 3200) ? RotateRef : (js_x < -3200) ? -RotateRef : 0;

    if (js.getButtonValue(0)) {
      // 並進移動(前進)
      ref_straight = StraightRef;
    } else if (js.getButtonValue(3)) {
      ref_straight = -StraightRef;
    }
    if (js.getButtonValue(6) || js.getButtonValue(7)) {
      // 並進速度を倍にする
      ri.jet = true;
    }
  }
  if (ri.jet) {
    ref_straight *= 2.0;
  }
  long diff = ticks - pre_ticks;
  pre_ticks = ticks;
  now_straight = setVelocity(now_straight, ref_straight, StraightAcc, diff);
  now_rotate = setVelocity(now_rotate, ref_rotate, RotateAcc, diff);

  ri.straight = static_cast<int>(now_straight);
  ri.rotate = VXV::Direction::rad(now_rotate);
  return ri;
}


// ロボットの制御
void robotCtrl(mRunCtrl& run, const robotInput_t& ri) {
  enum { RIGHT = 0, LEFT = 1 };

  // 並進速度、回転速度、トレッドから車輪毎の目標回転速度を計算
  int whl_mm_vel[2];
  int rotate_mm_vel = static_cast<int>(BODY_TREAD_MM * ri.rotate.to_rad() / 2);
  whl_mm_vel[RIGHT] = ri.straight + rotate_mm_vel;
  whl_mm_vel[LEFT] = ri.straight - rotate_mm_vel;

  for (int i = 0; i < 2; ++i) {
    run.setWheelVel(i, whl_mm_vel[i]);
  }
}

人が遠隔で操作

人が遠隔で操作する場合、モニタ機能のロボット位置表示を用いれば、通信遅延の影響を受けないロボット位置表示を行うことができる。これは、モニタライブラリが走行系の実デバイスと、表示用のシミュレータロボットに対して同時に移動コマンドを送信するためである。むしろ、実環境と表示情報の差異は、移動コマンド送信時における通信遅延時間と、ロボット側の通信処理時間に依存する。

ビーゴとシミュレータにコマンドを同時送信

ただし、モニタ機能の表示位置は実デバイスからの推定自己位置で上書きされるため、受信時の実機とシミュレータとの誤差は累積しない。

ビーゴからの自己位置でシミュレータ位置を上書

ここで、遠隔通信時における移動コマンドの特徴について確認する。遠隔ロボットの操作における主な問題は、

通信遅延
通信断絶

である。通信遅延から生じる具体的な問題は、

移動指示の到達に時間遅延
位置情報の到達に時間遅延

であり、モニタの表示機能を用いることでこれらの影響は緩和できる。
通信断絶による問題点は、

以後の操作が不可
最後に出した移動コマンドが評価され続ける

となる。
特に、ビーゴの走行コマンドは移動コマンドと監視コマンドを繰り返す形式を取るため、遠隔操作においては Watch dog timer の導入は必須である。

モニタ機能を利用する場合の通信形式は以下のようになる。

通信方法と設定

これより、TCP/IP 通信の箇所では通信のタイムアウト時間、リトライ回数が指定できればよく、同時に Watch dog timer が走行制御コントローラにて動作すればよいと言える。上記要件を実装したプログラムを以下に示す。

操作側で実行

/*
  遠隔操作用のクライアントプログラム
  Satofumi KAMIMURA
  $Id$
*/

#include "robotServer.h"
#include "mRunCtrl.h"
#include "mURGCtrl.h"
#include "joystickCtrl.h"
#include "inputHandle.h"
#include <stdlib.h>


int main(int argc, char *argv[]) {
  try {
    char* host = NULL;
    for (int i = 1; i < argc; ++i) {
      if (!strncmp("--host=", argv[i], 7)) {
        host = &argv[i][7];
      }
    }
    if (!host) {
      printf("specify connection host: --host=<host name>\n");
      exit(1);
    }

    mRunCtrl run;
    mURGCtrl urg;
    vmonitor::connect(argc, argv);
    if (run.connectSocket(host, RunVirtualPort) < 0) {
      printf("RunCtrl::connectSocket: %s\n", run.what());
      exit(1);
    }
    if (urg.connectSocket(host, URGVirtualPort, true) < 0) {
      printf("URGCtrl::connectSocket: %s\n", run.what());
      exit(1);
    }

    // 通信設定
    enum { TcpipTimeout = 5000 };
    run.set_sendRetryTimes(0);
    run.set_recvTimeout(TcpipTimeout);
    urg.set_recvTimeout(TcpipTimeout);
    run.set_watchDogTimer(TcpipTimeout + 1000);

    urg.setOwnCrdToObject(&run);
    run.adjustSubTreeTicks(0);

    vmonitor::show();

    // Joystick があれば初期化
    JoystickCtrl js;
    if (js.size() > 0) {
      js.activate();
    }

    // メインの制御ループ
    UserInput::userInput_t ui;
    int pre_times = -1;
    do {
      ui = UserInput::getInputed();
      robotInput_t ri = inputHandle(js, ui);
      drawJetMode(ri);
      robotCtrl(run, ri);

      // 環境情報の取得
      int times = urg.capture();
      if (times != pre_times) {
        drawCaptures(urg, run);
      }
      VXV::Delay(1);
    } while (!ui.quit);
  } catch (std::exception& e) {
    printf("exception: %s\n", e.what());
  }
  return 0;
}

ロボット側で実行

/*
  遠隔操作用のサーバプログラム
  Satofumi KAMIMURA
  $Id$
*/

#include "robotServer.h"
#include "connectionHub.h"
#include "runCtrlHub.h"
#include "timeUtils.h"


int main(int argc, char *argv[]) {
  try {
    RunCtrlHub run_port(RunVirtualPort, "/dev/ttyUSB0", 115200);
    ConnectionHub urg_port(URGVirtualPort, "/dev/ttyACM0", 115200);

    printf("begin....\n");
    while (true) {
      if (!run_port.update() || !urg_port.update()) {
        break;
      }
      VXV::Delay(1);
    }
    printf("disconnected\n");
    
  } catch (std::exception& e) {
    printf("exception: %s\n", e.what());
  }
  return 0;
}

以上です。

ソフトウェアが操作

最終的には、ロボット上のソフトウェアが環境センシングした結果を基に動作することを目指す。
今回は、教示再生を行うプログラムを作成してお茶を濁す。

!!! logfile.xml からどうにかする

!!! 実行時に覚えておく

つか、本当に作るのか？