2017-03-29

gazebo、ros_controlについて

オリジナルロボットを作ってgazeboでシミュレートする方法は下記のページがわかりやすい。

ROSで始めるロボティクス（3）ー差動二輪ロボットを準備する ~ BRILLIANTSERVICE TECHNICAL BLOG

gazeboとROSの連携について

Gazebo + ROS で自分だけのロボットをつくる 5. GazeboとROSの連携 - Qiita

Controller と HardwareInterface との間の処理の仕組み（1. ロボットモデルの定義と登録） - Qiita

No.6-5:GazeboをROSに繋ぐ (GazeboにおけるURDF 編) - 九州工業大学 CIR-KIT Blog

ros_controls/ros_controllers の制御の仕組み (position/effort/velocity_controllers の基礎) - Qiita

このあたりの記事が詳しいが、詳しすぎてよくわからない。

ros_controlのコンセプトは↓ここに書いてあるぽい

http://roscon.ros.org/2014/wp-content/uploads/2014/07/ros_control_an_overview.pdf

オリジナルロボットのためのurdfファイルやyamlファイルやlaunchファイルを見て、その書式を見て、意味を解読することをやると理解できるかもしれない

「ROSで始めるロボティクス（３）」の例についてみていく

launchファイルについて

gazebo.launchとcontrol.launchがある

gazebo.launchでは

/opt/ros/kinetic/share/gazebo_ros/launch/empty_world.launchを読み込み、gazebo_rosパッケージのspawn_modelというtypeの、urdf_spawnerという名前のノードを立ち上げている。urdf_spawnerのargsで、-param robot_descriptionとしているが、これは、my_robo.urdfを読んでいることになっていそう。

control.launchでは

まず、controller_managerパッケージのspawnerでjoint_state_controllerとdiff_drive_controllerを起動している。joint_state_controllerが常に必要なのかは不明。ROSの、joint_state_controllerのページはほとんど中身がない。

次にrobot_state_publisherを起動している。robot_state_publisherの説明によれば、robot_descriptionパラメタで指定されたロボットのURDFファイルを読んでjoint_statesというトピックを読んで、フォワードキネマティクスを解いてtfを発行しているとある。control.launchではrobot_descriptionパラメタは設定されていないが、（gazebo.launchでは設定されている）どのようにしてURDFファイルを特定しているのか不明。発行するトピックの名前はおそらく/tfと/tf_static。（名前空間の指定は出力されるトピックには影響しないのかな・・）

config/controller.yamlファイルについて

diff_drive_controllerの解説↓で、yamlファイルをどのように書くかの例が示されている

diff_drive_controller - ROS Wiki

hardware_interface::RobotHWSim、Joint State Interface/Joint Control Interfacesとは何なのか・・？

たぶん、ROSのノードから指令を受け取った後、HWまたはシミュレータに指令を出すまでの間で処理している部分のパーツ。

よく見たら、「ROSで始めるロボティクス（３）」の例がうまく動いてない。/my_robo/diff_drive_controller/*のトピックがrostopic listで見えない

rosrun controller_manager spawner diff_drive_controller

とやって、単体でやってもだめ。（正常な時に単体でも動くのかは不明）

apt-get install ros-kinetic-gazebo-ros-controlをしていなかったせいだと判明。

How to run controller_manager - ROS Answers: Open Source Q&A Forum

このロボットについてはdiff_drive_controllerがいてそれが/my_robo/diff_drive_controller/*のトピックを受け付けているっぽいが、

roslaunch kobuki_gazebo kobuki_playground.launchとしたときはどういったコントローラが/mobile_base/commancs/velocity等を受け付けているのか？

「ROSで始めるロボティクス（３）」とは違って、controllerというような名前の付くものをlaunchファイルで起動したりしていないように見える。。empty_world.launchが唯一怪しいか。それともなんとかcontrollerというのは起動しなくてもいいケースがあるのか？

kobuki_playground.launchの場合は、kobuki_gazebo.urdf.xacroの中で<gazebo><plugin name="kobuki_controller" filename="libgazebo_ros_kobuki.so"としている一方で、my_robo_description gazebo.launchの場合はmy_robo.urdfの中で<gazebo><plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"となっているので、kobuki_playground.launchのほうはros_controlを使っていない。

kobuki_playground.launchもgazebo.launchもどちらも、/gazeboと/*/robot_state_publisherの両方から/tfが発行されている。robot_state_publisherは必要なのかな・・？

ros_controlでノード間通信に依らずロボット間のハードウェアの違いを吸収する - ゼロから始めるロボットプログラミング入門講座

この記事によると独自のコントローラを作るにはこちらが参考になるそう

controller_interface · ros-controls/ros_control Wiki · GitHub

hardware_interfaceについてはこちら

hardware_interface · ros-controls/ros_control Wiki · GitHub

2017-03-27

gazebo上のkobukiシミュレートで衝突時にバンパのイベントが発生しない

私の環境（Ubuntu 16.04 + Kinetic、Thinkpad T410s)だと

$ roslaunch kobuki_gazebo kobuki_playground.launch

を実行し、小倉さんの本のros_startのvel_bumper.pyを実行してkobukiをブロックに衝突させても/mobile_base/events/bumperが発行されない

$ rostopic pub -r 1 /mobile_base/events/bumper kobuki_msgs/BumperEvent -- '1' '1'

として、/mobile_base/events/bumperを発行したらちゃんとkobukiがバックする。

/mobile_base/sensors/imu_dataや、/joint_statesはちゃんと発行されている。

過去のブログだと

ROS勉強 - akak1のブログ

他の人も同様の問題に行き当っているぽい

Bump Sensors on Simulated TurtletBot in kinetic - ROS Answers: Open Source Q&A Forum

ここにも同様の人が

How can I access the bumper sensors on a turtlebot in a simulation? - ROS Answers: Open Source Q&A Forum

2017-03-25

ROSのテストについて

pythonのunittestやMock、gmock、ROSについて

gmockとgtestのインストールは

$ sudo apt-get install lcov
$ sudo apt-get install google-mock

Google Test Install: まあ、こんなもんか

これを参考に。でもsudo cpはしないでおいた。

mocks関連でcatkin_makeができなかったものについて、CMakeLists.txtを編集してビルドが通るようにした

pythonのunittestについて

Pythonでテスト連載(2) ユニットテストの書き方 | UNITED アドテクブログ

pythonのmockについて

[python] まだmockで消耗してるの？mockを理解するための3つのポイント - くろのて

rostestについて

「rostest」の使い方 - うごくものづくりのために

2017-03-21

Win32APIや.NETって？

Win32 APIと.NET Frameworkの違いを教えてくださいあとGUIアプリを作る... - Yahoo!知恵袋

Windowsアプリケーションについての整理ー Visual Studio、Win32（Win32API）、.NET、.NET Framework、MFC、ATL、Windows SDK） - Awesome Hacks!

2017-03-13

RPiでRAMディスクに画像保存＆別マシンから1fps程度の画像閲覧

RAMディスクを用意

Raspberry PiのSDカード/マイクロSDカードの寿命を延ばす | ものづくりエクスペリメント

これをみてdphys-swapfileをapt-get remove

スワップファイルというのは

スワップとは｜swap ：意味/定義 - IT用語辞典

課題とは関係ないけれどもSDカードの寿命を伸ばすために、/tmp等をRAMディスクにするというのもやっておいたほうがよさそう。これはあとでやる。

SDカードをすべてリードオンリーにするということもできる。これはシステムが出来上がってからやったらよいのかもしれない。

Raspberry Pi でfsprotectを試す | よもやま雑記帳

RAMディスクをどこにどう作るか

Linux Tips – RAMディスク（/dev/shm）のIO性能をチューニングに活用する方法

Linuxキーワード - RAMディスク：ITpro

このへんを見ると/dev/shmをそのまま使ってもよさそう

新たに作るならこれが参考になりそう

マイクラ鯖に持って来い!!、LinuxでRAMディスク!! | 純規の暇人趣味ブログ

シャットダウン時にデータが消えないようにSDカードに書くという動作を加えるというやり方も書いてある。電源が急に切れた場合に対応するにはcronで一日おきにsdカードに保存するなど。

Raspberry Pi 3にssh接続

connection refusedと出たが、下記を見て、raspi-configを実行してsshを有効にしたら接続できるようになった

pi 3 - No SSH (Connection refused) using raspbian with pixel - Raspberry Pi Stack Exchange

USBカメラ画像を取得して保存&ソケットで画像を飛ばす

Cでデバイスファイル開いて扱ってもいいが、 OpenCVを使うことにする。

Cプログラミングによる画像処理(1)

pythonでやることにする。

$ sudo apt-get install libopencv-dev

$ sudo apt-get install python-opencv

Raspberry Pi で OpenCV（リベンジ） - Qiita

ここのテストプログラムは動いた。

Pythonによる通信処理 - Qiita

これでとりあえずソケットの形に。複数接続するには

socketプログラムの初歩から初める - Keep on moving

threadを使う
forkする
selectなどを使う

の三つがあるそう。selectを使うのはこんな感じ

Pythonでネットワークプログラミング | saito's memo

windows側からpython実行するのに下記のやり方をググった。

import os

os.chdir("C:\\ほげほげ")

os.getcwd()

execfile("hoge.py")

windowsのpythonにnumpyをインストール

WindowsでPython3, numpy, pandas, matplotlibなどインストール - Qiita

> python -m pip install numpy

これでインストールできた

import cv2.cv as cvではなくimport cv2だけで使えるように下記のを参考

Python+OpenCVでカメラキャプチャ - Qiita

ソケットで画像を飛ばすのは下記

OpenCVでRaspberry Piから画像を転送する - kivantium活動日記

ソケットで接続するWindows側クライアント

クライアントはCSでwinのformで作る。文字列にエンコードしたjpegをデコードする。

TCPクライアント・サーバープログラムを作成する: .NET Tips: C#, VB.NET

デリゲートについて

デリゲート - C# によるプログラミング入門 | ++C++; // 未確認飛行 C

マルチスレッドについて。threadやtaskやasync/awaitを使う方法があるらしい。このうちTaskと、async/awaitが現行のやり方でそれ以外は古いらしい。

ThreadじゃなくTaskを使おうか？ - Qiita

Taskを極めろ！async/await完全攻略 - Qiita

できる！C#で非同期処理(Taskとasync-await) – kekyoの丼

マルチスレッド - C# によるプログラミング入門 | ++C++; // 未確認飛行 C

書き方の例

[C#]ざっくりマルチスレッド(非同期処理) – gomokulog

[C#]async/awaitの使い方メモ、その１。 | Kimux.Net

別スレッドからフォームのコントロールにアクセスするのはスレッドセーフではないらしい

方法 : Windows フォームコントロールのスレッドセーフな呼び出しを行う

排他処理のやり方も気を付ける必要があるらしい

.NET TIPS：非同期：awaitを含むコードをロックするには？（SemaphoreSlim編）［C#、VB］ - ＠IT

→timerを使えば解決

だけど、スレッドを使えるようになりたいのでそれはそれであとで勉強しておく

複数カメラの接続はserver*.pyを複数立ち上げることで実現することにする。シェルスクリプトでやる。

pgrepという便利コマンドがある

pgrep, pkill を使用してプロセスを殺す - 技術メモ帳

kill `pgrep -f hogehoge`とpkill -f hogehogeとは同じかもしれない。

ソケットでの画像の送受信のやり方について

受信バッファの準備ができないうちに送信したり、受信しないうちに受信バッファを読み込みに行ったりしているのではないか。その問題を解決するのに、１）受信バッファのサイズを十分な大きさに指定する　２）アプリケーションレイヤでハンドシェークする　というののいずれかをすればよいだろうということでこれの様子をみる。

１）受信バッファのサイズを指定する

Win32APIの（？）Winsockで、ポートごとのバッファの設定ができるのか調べた。

setsockopt function (Windows)

これのSO_RCVBUFが関係しそう

受信バッファ - odawaraの「はてな de メモ」

そもそもソケット通信がよくわかってない

ƒ\ƒPƒbƒg

バイナリとアスキーではどちらのほうがサイズが大きいのか？→パッと調べたところではバイナリのほうがデータが大きい場合もあるようなので、ファイルの詳細な形式によるということだと思う。

SocketのReceiveBufferSizeプロパティでbufferサイズはいじれる模様

Socket.ReceiveBufferSize プロパティ (System.Net.Sockets)

INIファイルの読み込み

C#.NET Tips

サーバ側でカメラをホットプラグにする処理

opencvのエラーをキャッチできないためにカメラを抜いた状態で

g_capture.read()をしてもret1やframeが直前の状態（ret1 = True）をキープしてしまい、カメラの接続が外れたことを検知できない。

しかたないので、pythonからシェルスクリプト実行して/dev/video*の存在を見ることでデバイスの接続有無を判定するようにした

サーバ側の画像保存を厳密に一定時間になるように時間の制御を加えた

Rapsberry piのカメラ画像の表示処理結果に遅延→カメラ画像取得に遅延？

OpenCVでバッファリングしてるところがあるのでそこでのバッファをしないという風にする↓の方法を試してみる

OpenCV カメラの遅延の対策 | ぴよちゃんの工房

cap.get(cv2.cv.CV_CAP_PROP_FPS)してみたら

HIGHGUI ERROR; V4L2; unable to get proprty <unknown property string>というエラーで-1の値が返ってくるので、この方法が使えないポイ。

はんだ日和: Raspberry Pi でOpenCVのサンプルを動かす（カメラと顔認識）

ここに書いてあるような、CaptureFromCAMをして得られるCaptureオブジェクトに対しても同様に、CV_CAP_PROP_FRAME_WIDTHはちゃんとgetできるが、CV_CAP_PROP_FPSはgetできない。何の仕様が違うのかな、、ともあれ仕方ないので、やはりOpenCVのソースをいじるしかないのかな？

今はOpenCV 2.4.9を使っているが、OpenCV 3.xなら解決しているかもしれない

OpenCV: C API for video I/O

上記のURLをみると、三通りの方法がある。

１．CV_CAP_PROP_BUFFERSIZEというパラメタを使う

２．フレームのクエリに必要な時間を計測し、その時間がある値以上であれば読み捨てる（バッファからのクエリはそうでない場合よりも時間がかかるので）

３．別スレッドを立てて画像を読み捨てさせてバッファを空にする

CV_CAP_PROP_BUFFERSIZEは上記のとおりそんなオプションはないと出るので、２．の方法をトライしたがうまくいかない。単純に２，３フレーム読み捨ててから使うようにしたら遅延がなくなったので、３．の方法を参考に、別スレッドを立てて画像を読み込む専用のスレッドと、表示と保存をするスレッドを用意するのがよさそう。

ちなみにバッファからの読み込みは20msec程度。（でも測り方によっては30msec程度）

結局、表示と保存と読み込みの三つにスレッドを分けた。