connectById.cpp

Sample of connect to URG using by serial ID

Author:

Satofumi KAMIMURA

Id:

connectById.cpp 1737 2010-03-06 07:15:19Z satofumi

#include "mUrgDevice.h"
#include "UrgUtils.h"
#include "Connection.h"
#include "ConnectionUtils.h"
#include <SDL.h>
#include <iostream>

using namespace qrk;
using namespace std;


namespace
{
  bool adjustById(mUrgDevice* urg[], long serials[], size_t n)
  {
    // シリアル ID を確保
    vector<long> device_serials;
    for (size_t i = 0; i < n; ++i) {
      long serial_id = urgSerialId<mUrgDevice>(urg[i]);
      device_serials.push_back(serial_id);
    }

    // URG オブジェクトが シリアル ID で指定した順番になるようにする
    for (size_t i = 0; i < n; ++i) {
      long requested_serial = serials[i];
      if (device_serials[i] == requested_serial) {
        continue;
      }

      // 目標の ID があるインデックスを探して交換
      vector<long>::iterator p =
        find(device_serials.begin(), device_serials.end(), requested_serial);
      if (p == device_serials.end()) {
        return false;
      }
      size_t swap_index = p - device_serials.begin();

      swapConnection<mUrgDevice>(*urg[i], *urg[swap_index]);
      swap(device_serials[i], device_serials[swap_index]);
    }

    return true;
  }
}


int main(int argc, char *argv[])
{
  // 接続させたい URG の シリアル ID を順番に記述する
  long serials[] = {
    614967,
    710002,
  };
  const char* devices[] = { "/dev/ttyACM0", "/dev/ttyACM1" };

  // URG オブジェクトの初期化
  mUrgDevice urg_0(argc, argv);
  mUrgDevice urg_1(argc, argv);
  mUrgDevice* urg[2];
  urg[0] = &urg_0;
  urg[1] = &urg_1;

  // 接続
  size_t n = sizeof(serials) / sizeof(serials[0]);
  for (size_t i = 0; i < n; ++i) {
    if (! urg[i]->connect(devices[i])) {
      cout << "UrgDevice::connect: " << urg[i]->what() << endl;
      exit(1);
    }
  }

  // 指定した シリアル ID とオブジェクトが一致するように調整
  if (! adjustById(urg, serials, n)) {
    cout << "fail swapById()" << endl;
    exit(1);
  }

  // 調整結果を表示
  cout << "result" << endl;
  for (size_t i = 0; i < n; ++i) {
    cout << i << ", " << urgSerialId<mUrgDevice>(urg[i]) << endl;
  }

  // !!!

  for (size_t i = 0; i < n; ++i) {
    delete urg[i]->connection();
    urg[i]->setConnection(NULL);
  }

  return 0;
}