読者です 読者をやめる 読者になる 読者になる

cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

OpenSoundControlライブラリのlibloを便利に使えるように、クラスのメンバー関数をコールバック関数に登録できるようにする

(編集中)

ネットワークの設計方針

ロボットの遠隔操作の通信系をどのように設計するべきかを考えた場合、トラブルに頑健であることが最優先事項に挙げられます。ネットワークの頑健性は、

  1. 接続が切れにくい
  2. 接続が切れても復帰できる
  3. データが一部欠損しても大丈夫
  4. 他のプロセスへ与える負荷が少ない

などが考えられます。デジタルテレビ放送などがすごい情報量を送ってきていることを考えると、きっとネットワークの専門家の方々はとても良いネットワークシステムを既に開発されているのだと思いますが、一般ユーザが使えるのは、いまだTCP/IPもしくはUDP/IPを基本とした原始的なものが普通です。

TCP,UDPを便利に使えるようなライブラリは沢山開発されていますので、その中からよさそうなものを選ぶことにします。ソケット通信は基本ですが、書式や再接続の手間が面倒くさくて、複雑なことをする気になれません。CORBAはもう、いろいろ勘弁してください。XML-RPCは書式が簡単で良いのですが、遅いのと、タイミングの制御がしずらい、という問題があります。

OpenSoundControl

そこで、以前ちらっと紹介したOpenSoundControl(以下、OSC)の出番となります。詳しい説明は、下記の福地先生のWEBページを参照してください。

http://fukuchi.org/oss4art/Open_Sound_Control

OSCの何が良いかというと、

・小さいOSCパケット単位だけで送ることを最初から考慮しているので、ネットワークが切れても、再接続が容易。(明示的に書く必要が無い)

・送受信のプログラムが比較的容易に書ける

・OSCアドレスがURL風で階層構造を書きやすく、ロボットをオブジェクト指向的に見たときに親和性が良い

・もともと音データの送受信のために開発されているので、リアルタイム性が期待できる

・ライブラリ実装によってはTCP、UDPとも対応できる。

liblo

OSCは通信仕様であって実装ではありません。さまざまな実装がありますが、UDPだけでなく、TCPが使えてc言語で書けるという理由からLibLoを使うことにします。LibLoの説明も下記リンクを見てください。

http://fukuchi.org/oss4art/Liblo

 

libloにc++のインターフェースを付けて、大きなバイナリーデータを受信するクラス

ただし、Libloはc言語のライブラリで、しかもコールバック関数を使っているので、c++風に書こうとすると少し工夫が必要になります。

下記に、参考用ソースコードKinectのポイントクラウドデータをSnappyで圧縮したものが送られてくるので、それを受け取って解凍するコードの例。

static宣言したメンバー関数をコールバック関数に登録します。

そのコールバック関数の中では、引数で渡したthisポインタから仮想宣言した関数を呼び出します。

継承先のクラスに仮想宣言した関数の実態を書いてやります。

なんか無駄に、ややこしいですね。ただ一度書いてしまえば、あとは中身を少し書き換えるだけで便利に使えます。ただ、現状の注意として、おそらくlocalhost相手の通信でしか下記のコードは思うように動きません。

OSCReceiver.h

#include <stdio.h>

#include <stdlib.h>

#include <algorithm>

#include <string>

 

#include "snappy.h"

#if _DEBUG

#pragma comment(lib, "C:\\workspace\\snappy-1.1.1\\snappy-msvc\\Debug\\snappy.lib")

#else

#pragma comment(lib, "C:\\workspace\\snappy-1.1.1\\snappy-msvc\\Release\\snappy.lib")

#endif

 

#include "lo/lo.h"

#if _DEBUG

#pragma comment(lib, "C:\\workspace\\liblo-git\\lib\\DebugDLL\\liblo.lib")

#else

#pragma comment(lib, "C:\\workspace\\liblo-git\\lib\\ReleaseDLL\\liblo.lib")

#endif

 

class OSCReceiver

{

public:

  lo_server_thread st;

  int LO_PROTO;

  std::string PORT; 

  std::string address;

  int done;

 

  OSCReceiver(){

    LO_PROTO = LO_TCP;

    PORT = "7770";

    address = "/kinect/pcd"; 

    done = 0;

  }

 

  void init() {

    st = lo_server_thread_new_with_proto(PORT.data(), LO_PROTO, error);

  }

 

  void regist_functions() {

    lo_server_thread_add_method(st, address.data(), "i", s_func_header, this);

    lo_server_thread_add_method(st, address.data(), "bi", s_func_handler, this);

    lo_server_thread_add_method(st, address.data(), "c", s_func_footer, this);

    lo_server_thread_add_method(st, "/quit", "", quit_handler, this);

  }

 

  int start() {

    lo_server_thread_start(st); 

    while (!done) {

#ifdef WIN32

       Sleep(1);

#else

       usleep(1000);

#endif

    }

    lo_server_thread_free(st);

    return 0;

  }

 

  static int s_func_header(const char *path, const char *types, lo_arg ** argv,

int argc, void *data, void *user_data){

    OSCReceiver* p = (OSCReceiver*)user_data;

    return p->func_header(path, types, argv, argc, data, user_data);

  }

 

  virtual int func_header(const char *path, const char *types, lo_arg ** argv,

int argc, void *data, void *user_data) = 0;

 

  static int s_func_handler(const char *path, const char *types, lo_arg ** argv,

int argc, void *data, void *user_data){

    OSCReceiver* p = (OSCReceiver*)user_data;

    return p->func_handler(path, types, argv, argc, data, user_data);

  }

 

  virtual int func_handler(const char *path, const char *types, lo_arg ** argv,

int argc, void *data, void *user_data) = 0;

 

  static int s_func_footer(const char *path, const char *types, lo_arg ** argv,

int argc, void *data, void *user_data){

    OSCReceiver* p = (OSCReceiver*)user_data;

    return p->func_footer(path, types, argv, argc, data, user_data);

  }

 

  virtual int func_footer(const char *path, const char *types, lo_arg ** argv, int argc, void *data, void *user_data) = 0;

 

  static void error(int num, const char *msg, const char *path){

    printf("liblo server error %d in path %s: %s\n", num, path, msg);

    fflush(stdout);

  }

 

  static int quit_handler(const char *path, const char *types, lo_arg ** argv,

                 int argc, void *data, void *user_data)

  {

    OSCReceiver* p = (OSCReceiver*)user_data;

    p->done = 1;

    printf("quiting\n\n");

    fflush(stdout);

     return 0;

  }

};

SnappyReceiver.h

class SnappyReceiver : public OSCReceiver 

{

public:

  std::string compressed;

  std::string uncompressed;

 

  int func_header(const char *path, const char *types, lo_arg ** argv,

int argc, void *data, void *user_data) {

    SnappyReceiver* p = (SnappyReceiver*)user_data;

    p->compressed.clear();

    p->compressed.reserve(argv[0]->i);

    return 0;

  }

 

  int func_handler(const char *path, const char *types, lo_arg ** argv,

int argc, void *data, void *user_data) {

    SnappyReceiver* p = (SnappyReceiver*)user_data;

    unsigned char *ptr = (unsigned char*)lo_blob_dataptr(argv[0]);

    std::copy(ptr, ptr+lo_blob_datasize(argv[0]), std::back_inserter(p->compressed));

    return 0;

  }

 

  int func_footer(const char *path, const char *types, lo_arg ** argv,

int argc, void *data, void *user_data) {

    SnappyReceiver* p = (SnappyReceiver*)user_data;

    bool result = snappy::Uncompress(p->compressed.data(), p->compressed.size(), &p->uncompressed);

    if(result) printf("fin %d %d %d\n", result, p->compressed.size(), p->uncompressed.size());

    return 0;

  }

};

main.cpp

int main() {

  SnappyReceiver receiver;

  receiver.init();

  receiver.regist_functions();

  receiver.start();

  return 0;

}