Robotics勉強会資料

Transcript

1. Copyright© Pasona Inc. All rights reserved. 1 C++で動くロボットのテスト環境をPythonで作った話 株式会社パソナ DXテクノロジー本部 / X-TECH 第1エンジニア室 / Web/AIチーム 夏谷実
2. Copyright© Pasona Inc. All rights reserved. 会社紹介と自己紹介 2 株式会社パソナ グループ事業内容 ・エキスパートサービス(人材派遣) ・BPOサービス(委託・請負) ・HRコンサルティング ・教育・研修 ・グローバルソーシング(海外人材サービス) ・キャリアソリューション(人材紹介、キャリア支援) ・アウトソーシング ・ライフソリューション ・地方創生ソリューション 自己紹介 名前：夏谷実 FPGAを使った画像処理や組み込みプログラミングが好き。 画像処理からDeep Learningの世界に入り、最近はロボットのAI部分を開発中。 twitter: @natsutan はてな: natsutan
3. Copyright© Pasona Inc. All rights reserved. 今日話すこと 3 ピッキングロボット用のC++で書かれたライブラリのテスト環境を作った。 開発したプログラムはC++で記載されてlibrobot.soのように共有ライブラリで提供している。 モチベーション： ・ピックのバグをロボを動かす前に見つけたい。 ・いろいろ機能を足したときのデグレをロボを動かす前に見つけたい ・アルゴリズムのパラメータをロボを動かさずに調整したい ・ドキュメントの整備をしたい プロジェクトリーダーからの要求事項 ・テストはPythonから行いたい。 ・リリースするバイナリでテストをしたい。（再コンパイル不可） テストランナーはPython標準のpytestを使おう。 Pythonにctypesという他言語IFがあるからこれを使えば良さそう テスト環境ではカメラ（Realsense）が接続されていないので、事前に画像を用意してカメラの撮 影をしたことにする。 テスト環境かどうかの判断は環境変数で行う。環境変数VCAMRA=1で仮想カメラが有効にする。 https://www.irasutoya.com/2014/10/blog-post_46.html
4. Copyright© Pasona Inc. All rights reserved. はまったところ一覧 4 課題 番号 C++対応 1 enum 2 Deep Learning動作 3 Realsense のAPIが呼べない。 4 軌道の期待値一致 5 やってみたら結構苦労しました。 順番に説明します。
5. Copyright© Pasona Inc. All rights reserved. はまったところ１：PythonからC++の関数を呼び出す 5 最初の壁： Pythonのctypes は、C bindingであってC++のbindingではない。C++用のbindingは他にある。 例えば連続する座標を扱うようなstd::vector<float>が直接やりとりできない。 テスト環境 (python) pytest librobot.so ロボ制御ライブラリ (C++共有ライブラリ） DLLのload C++の関数呼び出し https://www.irasutoya.com/2014/09/blog-post_849.html 経緯： 元々中身はC++で開発していたが、APIはC言語のI/Fを公開していた。 あるとき、librobot.soを呼び出している方もC++で開発していることが分かった。 そこで、もうC言語にこだわらなくて良いよね、段階的にC++へ移行しましょうという話があった。 その後の社内レビューで、「Cの配列（実質はポインター）じゃなくて、std::vectorの参照に置き換 えろ」と指摘したの自分じゃん。
6. Copyright© Pasona Inc. All rights reserved. はまったところ１：PythonからC++の関数を呼び出す 6 Python librobot.so libc libapitset.so Test用ライブラリ この枠が一つのプロセス空間で動く bash Python Deep Learningは別のプロセス空間で動く テスト用のライブラリを一つ作る これが、Pythonとテストするライブラリのヘルパー関数を提供する。 libcないとnewできない。 ctypesはCの構造体をどう扱っているのかデバッガで確認してみた 謎のint値しか持ってない。16進だと0xDE631600。 これ間違いなくpointerでしょう。
7. Copyright© Pasona Inc. All rights reserved. はまったところ１：PythonからC++の関数を呼び出す 7 C++の変数を作る class Hand { Hand(…) { }; ・・・ ｝; このクラスをPython内でinstance化したい。 hand = dll.ct_Hand(type, name, 10.5) ③ターゲットのライブラリのコンストラクタ が呼ばれ、instance化を行う。 void * ct_Hand(int type, char* name, double v) { Hand *p = new Hand(type, name, v); return (void *)p; } Hand::Hand(int type, const std::string &name, double v) {type:type .. } hand = dll.ct_Hand(type, name, 10.5) ①Python側でヘルパー関数を呼び出す。 ②ヘルパー関数はCのI/Fを提供して、内部で newを使いinstance化を行う。 ④ヘルパー関数は、voidのpointerに変換して返す。 ⑤Pythonの中でC++の変数が使える pointer
8. Copyright© Pasona Inc. All rights reserved. はまったところ１：PythonからC++の関数を呼び出す 8 C++の関数を呼び出す。 int Pick(Hand &hand, Environment &env, int p) この関数をPythonから呼び出したい。 p = ct.c_int(10) return_code = dll._ZNxxPickREKN_20xx(hand, env, p) ②ターゲットのライブラリ内では、上手くポインターが渡っ て普通に処理ができる。 int Pick(Hand &hand, Environment &env, int p) { return PICK_OK; } return_code = dll._ZNxxPickREKN_20xx(hand, env, p) ①Python側でC++の関数をを呼び出す。 int等は、Cで渡せる型に変換する。 クラスの参照は、さっき作ったpointer値をそのまま渡せばよい。 マングリングに注意して、直接C++の関数を呼び出す。 ③戻り値intはそのままPythonで使える pointer
9. Copyright© Pasona Inc. All rights reserved. はまったところ１：PythonからC++の関数を呼び出す 9 C++のclassの情報を読み出す dump_hand(hand) ②ヘルパー関数内で、変数の情報を外部ファイルに書き出す。 void dump_hand(Hand *p) { //jsonに出力 fd << “¥”name:¥” << p->name; } hand_dic = json.load() ①Python側で値を読み出すためのヘルパー関数を呼び出す ③Python側でJsonファイルを読み込み、データを復元する。 class Hand { Hand(…) { }; ・・・ ｝; C++側で変更されたこのClassの変数の値を知りたい。 pointer Hand.json
10. Copyright© Pasona Inc. All rights reserved. はまったところ２：列挙体 10 C++の列挙体とPythonの列挙体の値を手で合わせてる。 これは手で合わせないといけない。他のcffiでも多分同じ。 C++のコードレビュー時の僕 テスト環境書くときの僕 今、ヘッダーファイルから列挙体だけパースして、値を割り当ててPythonのプログラムにするコンパイラを作成中。 enum HandType { FIRST_HAND, SECOND_HAND, FAST_HAND ｝; class HandType(Enum): FIRST_HAND = 0 SECOND_HAND = 1 FAST_HAND = 2 https://www.irasutoya.com/2015/12/blog-post_30.html 列挙体に値を割り当てない。 割り当てるとintとして扱う人が出てくる。 THIRD_HANDが出たときに、 SECOND_HANDの後に追加しても、一番最後 に追加しても問題無いようにしてください。 お願いだから自動で振ら せないで、全部値入れて 欲しい。 https://www.irasutoya.com/2014/09/blog-post_849.html
11. Copyright© Pasona Inc. All rights reserved. はまったところ3 11 Deep Learning動作 いろいろあって、C++からPythonで作ったNeural Networkを動かすのにboost::pythonを使っている。 これが、pytest->C++->boost.pythonの順番でよばれてプログラムがCoredumpする。 結果的に、librobot.soからNNを動かすのに4種類の方法が混在している。 この辺の詳細興味ある方はコメントとか、twitter @natsutanで言及してください。 需要有ればまた説明します。 pytest librobot.so boost::python venvで作ったPython interpreter Boostコンパイルじに指定した python interpreter テスト環境時はsystem関数を使う事で回避 system(“python3 infer.py”)
12. Copyright© Pasona Inc. All rights reserved. はまったところ4 12 実際のカメラが接続されて無くても、C++からlibrobot.soをLoadして、Realsenseの関数を呼んでもOK。 点群カメラ Realsense RenderPointMap アルゴリズム Pythonからlibrobot.soを呼び出して、そこからRealsenseの関数を呼んだらCoredump pytest librobot.so librealsence2.so これ自体はよくある話で慣れてきたが、librobot.soの最初を作った立場としては納得いってない。 当初、Realsense依存を無くすように全体の設計をしている カメラに依存しないコンテナを用意して、 後段に処理を渡す。 Realsenseの所を仮想カメラに置き換えたのに、そもそもなんでRealsenseの関数が呼ばれるんだ？という疑問
13. Copyright© Pasona Inc. All rights reserved. はまったところ4 13 その後、カメラがRealsenseだけになった。 その後から入ってきた人がいろんな所にRealsense前提の処理を追加していった。 点群カメラ Realsense RenderPointMap アルゴリズム テスト環境で仮想カメラを使うというのはカメラの追加変更に相当するが、せっかく前もって用意した仕組みが機能していなかった。 プロジェクトを進める上で、ライブラリの設計思想などがドキュメント化されておらず、後から来た人と共有できてなかった。 ただ、テスト環境を作る中でこの周辺で一つ潜在的なバグを見つけました。 時間があれば最後紹介します。 RealsenseのAPI 呼んじゃう Realsenseから取ったデータを 直接アルゴリズムで使う
14. Copyright© Pasona Inc. All rights reserved. はまったところ４ 14 テストの期待値としてロボの姿勢（ハンドの軌道）を入れたかったが、これは挫折。 ・Deep Learningのバージョンが変わると結果が一致しない ・librobot.so内部に軌道の範囲チェックが入っているのでそれを信じることに この軌道も期 待値一致の対 象にしたい gripper open
15. Copyright© Pasona Inc. All rights reserved. 成果 テスト環境を作って３シーン、６シナリオでテスト環境が出来ました。 ロボ動かす前にバグを２つ見つけた。一つは面白かったので紹介。 バグではないが設計上の課題は、結構見つけてます。 ドキュメントが整備できた。 今の所かけた工数分ペイしてないけど、これからガンガンバグをみつけてくれるはず。
16. Copyright© Pasona Inc. All rights reserved. 見つけたバグ 実機 仮想カメラ project_point_to_pixel プロジェクトの最初に作ったカメラに依存しな いアルゴリズムを使っている。 全てmmでのやりとりを想定している。データが mの場合は処理前に前にmmに変換する Realsenseは座標をmで返してくるので、mでのや りとりを期待している。mmのデータが来た場合は、 mに変換して関数を呼び出している。 実機で動いているから、こっちの動作が正しい。 RenderPointMap バグの症状：仮想カメラにするとなぜか動かない。 調べるとmとmmが違う。1000倍したり、1/1000すると上手く動く。 おかしくなるところは実際の座標から、画像上の位置（pixel）を求めるところ。 ただ、どこかを合わせると、別のどこかがまた1000倍されたりする。 この関数にmmの座標を与えても、それっぽく動く
17. Copyright© Pasona Inc. All rights reserved. 見つけたバグ mmの位置 mの位置 Realsenseの計算方法は、カメラの焦点を原点とする投影変換。 3次元の位置がN倍されても対応するピクセル位置は不変 mmでもmでもどちらを与えても正しいピクセル位置を返す。 顕在化してないバグ： mを期待している変数にmmが入っている。 mmで座標を返すカメラからmで座標を返すrealsenseに変更したときの直し忘れ。たまたま動いている。
18. Copyright© Pasona Inc. All rights reserved. まとめ 課題： 次はC++のbinding(pybind11)を使いたい。 テストの起動終了がpytest前提になってしまっていて、アルゴリズムの検証には使いにくい。 テストにいろんなファイルが必要でDocker化がやりにくい。 感想： C のポインターを使って、無理矢理動かすところは楽しかった。 テストがALL PASSすると嬉しい ２個だけどロボ動かす前にバグ見つけて嬉しい！ ドキュメントは面倒だったが、ここでやらないとだれもやらないので頑張った。 ご清聴ありがとうございました