へっぽこ元ロボコニスト

ロボコンに燃え尽きた自分が日々の出来事を書くだけのブログです

2018/11/11(Oculus GoとROSを通信させてLRFのデータ表示)

研究より趣味が好き

 

この夏にこの記事を見てOculus Goを買い,研究を一時そっちのけでVR開発をやっていました

qiita.com

 この人の記事はとても分かりやすく細かいところまでサポートしていて,この記事を書き上げた熱量には本当に頭が下がります

 

なので,この記事を参考にすれば基本的なことができますが,4か月過ぎた今少しバージョンアップされていてカメラのみでなくLRFの連携が含まれていたり,やり方が変わっているので補足的なことを書いてみたいと思います

 

インストールなどの開発環境は全て終わっているものとします

Unity 2017.4.1

使用するAssets

ROS# Version 1.3

github.com

Oculus Utilities for Unity 1.28.0

Oculus Utilities for Unity | Developer Center | Oculus

 

基本的な設定

PlatformをAndroidに変更

  1. File > Build Settingsを開き,Androidを選択してSwitch PlatformをクリックしてPlatformを変更

 

VRの設定

  1. Edit > Project Settings > Playerを選択
  2. XR SettingsのVirtual Reality Supportedにチェック
  3. Virtual Reality SDKsの欄にOculusを追加

 

Buildのための設定

  1. アプリのパッケージ名を設定
    Edit > Project Settings > Player > Other > Package Nameを、com.[CompanyName].[ProductName]の形で適当な組織名とプロダクト名に変更
  2. APIレベルを設定
    Edit > Project Settings > Player > Other Settings > Identification > Minimum API Level > API Level 25
  3. Scripting Runtime Versionを設定
    Edit > Project Settings > Player > Other Settings > Configuration > Scripting Runtime Versionを.NET 4.x Equivalentに変更

 

OculusUtilities Unityをインポート

  1. 解凍して,OculusUtilities.unitypackageをAssetsにインポート
    この時アップデートできるよと聞かれるので,更新する(更新しないとなんか動かなかった気がする)

 

ROS#をインポート

  1. 解凍してROS#をAssetsにインポート

 

Oculus用のカメラを設置

  1. HierarchyウィンドウからMain Cameraを削除。
  2. ProjectウィンドウからAssets/Oculus/VR/Prefabsを開き、OVRCameraRigをHierarchyウィンドウにドラッグ&ドロップ

 

 RosConnectorの設定

  1. Hierarchyウィンドウで右クリック > Create EmptyでGameObjectを作成し、名前をRosConnectorに変更。
  2. RosConnectorのInspectorウインドウを開き、Assets/RosSharp/Scripts/RosCommunicationからRosConnectorをドラッグ&ドロップ
  3. RosBridgeServerUrlを設定。
    ROS側のPCのIPアドレス(例:192.168.0.100)とポート番号(デフォルト:9090)をあわせて、"ws://192.168.0.100:9090"のように入力。

 

ここまでは@Spritaroさんの方法ですが,ここから少し違います

 

カメラ画像の受信

ImageSubscriberの作成

  1. Hierarchyウィンドウで右クリック > 3D Object > Planeで平面を作成する
  2. InspectorウィンドウでPositionを(0, 0, 10)に、Rotationを(90, 0, 180)に変更
  3. ProjectウィンドウのAssets/RosSharp/Scripts/RosCommunicationからImageSubscriberスクリプトを、RosConnectorのInspectorウィンドウにドラッグ&ドロップ
  4. ImageSubscriberのTopicを”/cv_camera/image_raw/compressed”に設定
  5. ImageSubscriberのMeshRendererに作成したPlaneをドラッグ&ドロップ

 

LRFデータの受信

Laser Scan Subscriberの作成

  1. ProjectウィンドウのAssets/RosSharp/Scripts/RosCommunicationからLaserScanSubscriberスクリプトを、RosConnectorのInspectorウィンドウにドラッグ&ドロップ
  2. LaserScanSubscriberのTopicを”/scan”に設定

 

LRFデータを表示するスクリプトはAssets/RosSharp/Scripts/RosCommunicationにMesh,Line,Spheresが用意されていますが,自分はMeshとSpheresができました

ここでは見栄えが良かったLaserScanVisualizerMeshを例に作成します

 

LaserScanVisualizerMeshの作成

  1. Hierarchyウィンドウで右クリック > Create EmptyでGameObjectを作成し、名前をLaserScanVisualizerMeshに変更
  2. InspectorウィンドウでPositionを(0, -3, 0)に、Rotationを(0, 0, 0)に変更
  3. ProjectウィンドウのAssets/RosSharp/Scripts/MessageHandlingからLaserScanWriterをLaserScanVisualizerMeshのInspectorウィンドウにドラッグ&ドロップ
  4. ProjectウィンドウのAssets/RosSharp/Scripts/SensorDataVisualizationからLaserScanVisualizerMesh.csをLaserScanVisualizerMeshのInspectorウィンドウにドラッグ&ドロップ
  5. RosConnectorに追加したLaserScanSubscriberのLaserScanWriterにLaserScanVisualizerMeshをドラッグ&ドロップ

 

これで実行すると,自分の足元にLRFのデータをMeshでつなぎ合わせた多角形が出てきます

f:id:masanari7430:20181111223852p:plain

ですが,これには少し問題がありLRFのデータ数が数百あると表示が全くできません

なので,LRFはURG-04LX-UG01の場合,urg_nodeの実行時に"cluster"を90ぐらいにして点群の数を8個にするとリアルタイムで再生できました

 

また,表示の向きをいくら変えてもカメラに対して常に水平のMeshが出来る上がるため,目の前に表示したいのですが出来ませんでした

 

後は@Spritaroさんの所を見れば,Oculus Goの向きを取得出来たり,カメラ画像を常に目の前に表示出来たりします

 

再度になりますが,この記事は@Spritaroの部分を引用した部分が多くあり申し訳なくありますが,LRFの表示に自分が何日も嵌った部分がありどうしても記事にしたくこの方法で書きました

 

ですので,これが誰かの参考になれば幸いです

 

 

 

 

 

 

 

2018/10/14(wiimoteを動かす)

ロボットの入力インターフェースとして、Willリモコンを使おうとした時の過程です

まずは、githubからソースを入手

git clone https://github.com/ros-drivers/joystick_drivers

 

しかし、

CMake Error at /usr/share/cmake-3.5/Modules/FindPkgConfig.cmake:578 (message):
None of the required 'libusb' found
Call Stack (most recent call first):
joystick_drivers/ps3joy/CMakeLists.txt:6 (pkg_search_module)


CMake Error at joystick_drivers/ps3joy/CMakeLists.txt:11 (message):
Failed to find libusb

 

libusbが見つからない

なんやそれと思って調べたらissueにあがってました

github.com

 

これに従って、インストール

sudo apt-get install libusb-dev

 

 

しかし、また次のエラー

 

fatal error: spnav.h: そのようなファイルやディレクトリはありません

 

これも調べると

github.com

 

これに従って、インストール

 

sudo apt-get install libspnav-dev

 

 

またエラー

 

fatal error: bluetooth/bluetooth.h: そのようなファイルやディレクトリはありません

 

これは、ここにのってました

 

github.com

 

インストール

sudo apt-get install libbluetooth-dev

 

次のエラーは

fatal error: cwiid.h: そのようなファイルやディレクトリはありません

github.com

インストール

sudo apt-get install libcwiid-dev

 

これでmakeができるようになりました

 

しかし、動かそうとした時に

Traceback (most recent call last):
File "/home/morishita/my_ws/src/joystick_drivers/wiimote/nodes/wiimote_node.py", line 93, in <module>
import wiimote.WIIMote
File "/home/morishita/my_ws/src/joystick_drivers/wiimote/src/wiimote/WIIMote.py", line 48, in <module>
import cwiid
ImportError: No module named cwiid

 

調べると、またissueが

github.com

インストール

sudo apt-get install python-cwiid

 

ここまで依存関係があったパッケージは初めてだったので書いてみました

 

 

2018/10/13(Think Pad X220のCPUが30%ぐらいになる)

 最近,ロボコンのためにThink PadのX220を使っているのですがなぜかCPUが常に30%ぐらいになっていました

 

タスクマネージャによると

「System」が30%もCPUを食っていました

さらにsystemの中でどのようなプロセスがCPUを食っているかを調べるために

「System」のCPU使用率が高い原因を調べる

ここで紹介されている「Process Explorer」を使って調べました

 

すると

「risdxc64.sys」というプロセスがCPUを食っていたので,これを調べていると公式のFAQで以下のようなページを見つけました

Ricoh Card Reader use 25% CPU - Lenovo Community

 

Ricoh Card Reader」これが悪さをしていました

これを無効化するには

バイスマネージャーの中の,記憶域コントローラの中の「Richo~」を無効化すれば治りました

f:id:masanari7430:20181013083615j:plain

さすがに中古PCでだいぶ古いのでこういう不具合も出ちゃうのかな

2018/7/20(インターンと論文の合間に研究について思うこと)

最近,メンタルが安定しない

 

英語論文を春休みに出しました

IEEEのなんか結構有名なところのものでしたが,先生のOSの中で審査されたので普通に通りました.

しかし,担当の先生に言われた「発表の時に苦しいかもね」この一言から休まらない日々が始まりました

最初は授業があり「授業があるから仕方ないな」という言い訳を心の中で言い訳にしてそこそこの進捗で満足していました

しかし,6月末になると急に来る焦り

7月末には完成版がいるのであと2週間しかない

ここから必死にしました

でも,結果は自分が満足いくところまでできませんでした

少し泣きました

 

親に久しぶりに電話して情けないようなことを言いました

「努力してもうまくいかない」

泣きました

 

結局,先生に新しい所は追加せずに最終原稿は出しますと連絡しました

「別にOKです」

と連絡が来ました

 

今これを書いているのはインターンの面接に行く途中に書いているのですが気づいたことがあって書きたくなりました

 

これって自分で勝手にハードル上げて,自分に絶望してるだけだなと思いました

だって先生はここまでしないとダメとか言ってませんから

 

正直,自分は自信があったんです

だって今ままで頑張ってきましたから

ロボ研や勉強,大学院試などいろいろ挑戦してきました

 

でも,周りにはそんなことしなくても研究成果が出てる人がいて,自分はその人達よりも出来る思い上がっていました

 

研究は時間がかかるものです

年単位で結果が出るのが普通で,それは大学の先生がガチでやってそれです

今日の講義では10年ほどかかってようやくできたロボットが紹介されていました

 

それを見てそんなものかと思いました

 

ただ学生はだからと言って手を止めては行けない気もします

でも,研究は先生の機嫌を取るためにするものでもない気がします

 

ただ,こんな感じで苦しい時間がありましたが,少しうれしいことがありました

 

第一志望のインターンが受かったこと

授業料免除されたこと

Oculus goが来たこと

 

この三つが苦しい時に来ました

少し気持ちが楽になりました

まだ行けるかなと

 

そんな感じで今から受かっても行くつもりのないインターン面接に行ってきます

2018/5/24(windowsでgstreamerを動かしたい)

参考になるところが皆無?なので書く

 

最近,gstreamerを使い始めた

ラズパイに接続したricoh thetawifi経由で転送してVRで見ることが目標

 

でも,その前にwifi経由でwindowsの方に持ってこないといけないと思い,gstreamerを使ってやってみた

 

Linuxのgstreamerの導入はとても簡単でapt-getでどうにかなる

qiita.com

ただ,問題はwindowsで何を入れればいいかわからなかった

以下は自分がした方法

  • まずは見やすいコンソール

windowsシステムツールのコマンドプロンプトはやりにくいと感じたのでcmderを導入

cmder.net

exeでアプリをインストールすることなくすぐ使えるので便利だし,見やすかった

 

  • gstreamerのインストール

公式から持ってくるがいつもはexeファイルでアプリをインストールするけど,msiファイルをダウンロードする

GStreamer: open source multimedia framework

f:id:masanari7430:20180524002401j:plain

また,色々あるが「devel」はおそらく開発途中の残骸で,「merge」は怖い?のでシンプルな名前のものを選んだ

そして,インストールするとき,「typical」の方法でやると接続されているデバイス中で容量が一番大きいものが指定される

(知らなくてcompleteでやった時,外付けHDDに入ってプログラムが迷子になった)

また,「complete」以外だとのちに出てくる必要なプラグインが入らないため

一度,「custom」で入れるとこを指定して戻り,「complete」でプラグインごと全部入れる

 

  • 実行方法

Linuxにrosを入れてやってた時はどこでもroscoreとかできるのでアプリを入れたらどこでも実行できると思っていたが違うかった

windowsではプログラムをインストールしたバイナリのファイルでコマンドを打ち込む必要があるのでそこに移動する

今回の自分ならここに移動して

f:id:masanari7430:20180524003227p:plain

「gst-launch・・・・」を実行した

 

やったのは,

qiita.com

qiita.com

でラズパイカメラとUSBカメラの二つをwindowsに転送してみた

ラズパイカメラは問題なく行けたが,USBカメラはlogicool製は問題なくできたが,BUFFALO製はなぜかできなかった

たぶんカメラによってはコマンドの変更が必要なのだろうけど,コマンド意味が全く分からないので対処が出来なかった

 

 

2018/4/23(春休みにしたこと①)

最近,ようやく時間が出来た

 

学会の原稿を2つ書いて春休みが終了しましたが,春休み中に

  • 高校の友達と旅行
  • HoloLens,pepper体験
  • ロボ研旅行
  • 初めてのリアル脱出ゲーム

の4つのイベントがありました

 

高校の友達と旅行

高校の将棋部のメンツで山口県へ旅行へ行ってきました

行ったのは

f:id:masanari7430:20180423223433j:plain

f:id:masanari7430:20180423224434j:plain

秋吉台の鍾乳洞は,2月に行ったのでかなり冷えてた気がします

たくさんライトアップされててよく見えていたのですが,ライトアップし過ぎでなんというか自然の作り出した自然物感が失われていたのが少し残念でした

カルスト台地は不思議で,写真ではカルスト地形が広がっていますが,少し左に向くと普通の山の森が見られます

本当に一部分だけに石灰岩が広がっています

  • 角島

f:id:masanari7430:20180423223518j:plain

f:id:masanari7430:20180423223749j:plain

天気が良くて角島への道がきれいに見えました

この山口県から角島へ行く道の横に休憩所と少し高くなった展望台があるのですが,僕たちが行ったときにカップルがいちゃついていたので死んでしまえと思いました

角島では友達が眼鏡を落とすハプニングが起きました

結局,彼の数万?の眼鏡は2度と見つかりませんでした

f:id:masanari7430:20180423223632j:plain

また来たよここ

中国地区の高専ロボコンを見に行った時に行ったのですがまた来ました

しかし,残念ながらこの日は有名な100種類ぐらいのソフトクリームを売っているお店には行けませんでした

  • カモンワーフ

f:id:masanari7430:20180423223732j:plain

f:id:masanari7430:20180423225131j:plain

昼ご飯のために寄りました

f:id:masanari7430:20180423223957j:plain

泊まる宿については何も知らなかったのですが,着いたら女将劇場なるものが毎晩やっているそうで,少し調べるとクレイジーな女将が何十年も一人芸を宿泊客に見せつけるものでした

しかも,月曜から夜更かしにも出たことがあったらしく面白そうで見に行きました

ネタはやはりおじいちゃんおばあちゃん向けのものが多く,分からないものもありましたがなかなかに楽しめました

 

 

2018/4/14(raspberry pi+ubuntu mateにおけるcatkin_makeのフリーズ)

久しぶりに書く

 

今日,論文の添削の合間にraspiにUbuntu Mateをインストールした実験用のraspiの用意をしていました

しかし,git cloneで必要なプログラムを入れてcatkin_makeしようとしたところフリーズしました

このフリーズは半年前から起こっていて毎回同じところで起きていました

今ままではプログラムのバージョンを落とすとうまく行っていたのですが,今回はこのフリーズを解消しようと思いました

 

色々試した結果,

catkin_make -j1 -l1

で解決しました

 

こうすると,raspiが本気を出さずにmakeするので,makeする時間は遅くなりますが,確実のmake出来るようになりました

 

たぶん,メモリか何かが高速でmakeしようとしていっぱいいっぱいになってフリーズしたのでしょう

2018/2/7(自主製作はもちろん失敗)

もちろん失敗

 

f:id:masanari7430:20180207225331j:plain

自主製作物が完成しました

100均で200円する腰のサポータを買ってきて取り付けました

これで自分の思い描いていた形はできたのですがやっぱり思い描いていたようには動きません

 

原因は

  • モータが剛体上に乗っていなので,駆動部からの反力に負けてモータがぐらぐらする
  • スライダの部分が思っていたような働きをしてくれない
  • お腹にサポータを巻こうとしていたが,腰の角度で位置が常に動くためリンク機構がそもそもうまく動かない
  • 駆動軸についているパーツが曲がって折れそうになる
  • ホルダーが大きくて動くとモータ部分と干渉する

少し動かしただけでこれだけ不具合があることが分かりました

 

なので,次のバージョンでは

  • 両軸を使って駆動する
  • 腰ではなく足にサポータを巻く
  • クランク機構ではなく違う機構を用いる
  • 小さい木の板をモータの下にひく
  • ホルダーの大きさに対して大きめのマージンを取った設計にする

ぐらいの改良を加えます

 

卒論がとりあえず書き終わったので,スライドを作りながら設計していきたいと思います

まずは,モータの下に木の板を加えますか・・・

 

今日,図書館アルバイトの面接に行ってきたのですが残念ながら落ちてしまいました

仕方がないので,塾講師もう少し続けますか

2018/2/2(自主製作物完成)

ようやく完成

 

さて前々から話していたものが完成しました

 

今日は卒論の提出日で朝に学科の方へ卒論を提出した後,前から考えていたものを作っていました

部室へ行き,ボール盤で角材を加工して,真鍮の加工は友達にお願いしました

(ただ,途中でドリルを折ってしまったのでめっちゃ申し訳ない気持ちになりました

ごめんなさい)

 

そして完成しました

f:id:masanari7430:20180202232801j:plain

何に使うかは秘密ですが,研究室の先輩に一発で当てられてしまって少しショックでした

上の写真は完成形ですが,駆動部だけ机に置いていたら一発で当ててきました

たぶん僕と同じものを考えていたのかなと思います

 

テスト駆動させて見ると,乾電池一本ならそこまで音は出なかったのですが,レギュレータだとかなり大きい音が出ました

 

また,放熱板付きのレギュレータが思っていたより発熱しました

マイコン用の電源から引っ張っているので,少し電流量が足りないかもしれません

 

さて,これを実際に駆動しようと思ったのですが,実はパーツが一つありません

なので,明日100均で買ってきます

明日が楽しみです

 

まあ,卒論が実は終わっていないのでやらないといけないのですが・・・・

2017/1/22(卒業お祝い基板)

やっと完成

 

半年ぐらい前から計画していた基板がようやく完成しました

今のB4のロボ研メンバーのうち2人が就職してしまうので,記念に何か作れないかなぁと思って計画していました

2人とも制御班なので,ロボ研のことを思い出すような基板が作れればいいなと思っていて,

表面にはロボ研のマスコットキャラクターのそうじまるを

f:id:masanari7430:20180122104042j:plain

裏面には,ロボ研の文字をLEDで描き,全員の名前をその下にシルクで描きました

f:id:masanari7430:20180122103718j:plain

ロボ研の文字はそこらへんにあるmicroUSBケーブルで光るようにしました

 

卒業が近くなってきて,卒論やら旅行の予定やらで忙しくなってきましたが,ちゃんと完成することができてよかったです.

 

あと,考えているものがあってあんまり人には説明できないのですが写真だけあげときます

f:id:masanari7430:20180122105035p:plain

そして,試作の部品を研究室の3Dプリンタで作ってみました

f:id:masanari7430:20180122105328j:plain

触ってみると思っていたより,リングの耐久度がないような気がしました

 

今週は卒論頑張ります

2018/1/18(STMメモ)

チップからSTM基板を作るときの最小構成を作るときのメモ

最低限JTAGポートをつなげれば書き込みができる

 

例としてSTM32F405xx407のデータシートを示す

ポート名/データシート/Kicad/

PA13/JTMS-SWSIO/SYS_JTMS-SWDIO

PA14/JTCK-SWCLK/SYS_JTCK-SWCLK

NRST/NRST/NRST

またNRSTピンはスイッチをデータシートのようにスイッチをつけるとリセットが容易に可能

 

BOOT0ピンは10kでプルダウン

あと傍に0.1uのコンデンサ

 

VCAPは2.2uのコンデンサをつける

 

VDDは電源だから3.3Vを入れる

VDDAはアナログ系の電源のだけど3.3Vでいい

 

VSSはグラウンド

VSSAはアナログのグラウンド(A/Dコンバータのグラウンド?)だけど基本的にVSSAも同じグラウンドにすればいい

 

VBATは

VBAT supply for RTC, 20×32 bit backup

と書かれているからとりあえず3.3Vをつける

 

電源に対するパスコンは,3つぐらいでいいかな?

 

 

2018/1/5(Think Pad x220の中古を買った)

あけましておめでとうございます

 

年末は研究室の最後の発表があったので忙しくて研究関係以外何も出なくて,ほとんど書くネタがありませんでした

でも,暇を見つけてやったことがあり

  • STMでMPU9250をI2Cでデータを取得する
  • 自作のSTMボードを作って,Lチカをする
  • 中古のThink Pad x220を買って,ロボコン用にカスタマイズ
  • ROSでシュミレーション環境を作る

ぐらいをやっていました

STM,ROS関係は後日書きますが,備忘録としてThink Pad x220を書きます

 

 購入した経緯

入学した当時に購入したノートパソコンは研究用にUbuntuを乗せたので,持ち運べるWindows機が無くなって困っていました

なので,新しいノートパソコンがいるなと思ってしました

ですが,何のノートパソコンを買おうかなと思っていた時に,TwitterのTLでThink Padがいいよっていうのを見かけたのでThink Padを買うことにしました

そして,値段も2万円以内を目標に探しているとThink Pad x220の中古をソフマップで1万6500円で見つけて買いました

 

届いたThink Pad

f:id:masanari7430:20180105231951j:plain

f:id:masanari7430:20180105232003j:plain

f:id:masanari7430:20180105232014j:plain

届いたThink Pad x220はきれいでキーボードのAとNが擦り切れてるぐらいでした

SSDは128GBで換装されており,メモリは2GB*2の4GBでした

バッテリーは6cellで100%まで充電したところ6時間持つので,ほぼ新品同様のバッテリーでした

 

なので,中古でバッテリーぐらいは新品にしないといけないなと思っていたのですが何も変えなくていいほどの良品が来ました

 

Windows10のインストール

windows10のインストールの前にリカバリーディスクの作成を行いました

搭載されているOSがwindows7 Professionalだったので,せっかくだからwindow7に戻せるようにリカバリーディスクの作成をしました

そしたら,DVD-RWの4.7GBが合計で3枚もいりました

 

そのあと,windows10のインストールメディアをUSBで作ってBootしてwindow10を入れました

30分ぐらいで入ったので拍子抜けしました

 

Think Padの目玉である,トラックパッドとホットキーと指紋認証はWindowUpdataをかけると自動で使えるようになりました

 

インストールしたもの

Git

Kicad

TrueSTUDIO

STMStudio

を入れました

Gitは家と研究室のデータのやり取りを容易にするため

Kicadは基本的にはデスクトップPCで回路は作りますが,はんだ付けの時に

TrueSTUDIOはST社がAtollicを買収したので将来的に見越してこれが使われるだろうと思い導入

STMStudioは,STMボードのデバックのためにインストールしました

 

しかし,残りのSSD容量が47GB/96.5GBと半分になってしましました・・・

 

まとめ

初めてThink Padを買いましたが,この値段でこのスペックは満足です

頑張ってSTMの開発します

 

 

 

2017/12/18(PCLライブラリでICPアルゴリズムをやってみた)

PCLライブラリを使ってみた

研究で,ICPアルゴリズムを使う必要があって,学部4年の自分の力では書けないことはわかっていたのでPCLライブラリを使ってやってみようと思いました

今回やったのは,2つのURGのデータをICPアルゴリズムを使ってどれだけ位置が違うかを取得します

コードは以下のようになります

#include <ros/ros.h>
#include <laser_geometry/laser_geometry.h>
#include <iostream>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>


class icp_transform{
  public:
    icp_transform();

  private:
    //pointcloud2を取得
    void cb_catch_cloud2_first(const sensor_msgs::PointCloud2::ConstPtr& cloud2);
    void cb_catch_cloud2_second(const sensor_msgs::PointCloud2::ConstPtr& cloud2);

    void transform(const ros::TimerEvent&);

    //ノードハンドラ作成
    ros::NodeHandle nh;

    ros::Publisher pub_pcl_first;

    ros::Subscriber sub_cloud2_first;
    ros::Subscriber sub_cloud2_second;

    //cloud2のデータ群
    sensor_msgs::PointCloud2 cloud2_first;
    sensor_msgs::PointCloud2 cloud2_second;

    //pcl_point_cloud
    pcl::PointCloud<pcl::PointXYZ> pcl_first;
    pcl::PointCloud<pcl::PointXYZ> pcl_second;
    //pcl::PointCloud<pcl::PointXYZ> pcl_second


    //時間の関数作成
    ros::Timer timer;

};
//コンストラクタ初期化
icp_transform::icp_transform(){
  pub_pcl_first = nh.advertise<pcl::PointCloud<pcl::PointXYZ> >("/pcl_data",10);

  sub_cloud2_first=nh.subscribe<sensor_msgs::PointCloud2> ("/cloud2_control_point_first", 100, &icp_transform::cb_catch_cloud2_first, this);
  sub_cloud2_second=nh.subscribe<sensor_msgs::PointCloud2> ("/cloud2_control_point_second", 100, &icp_transform::cb_catch_cloud2_second, this);

  timer = nh.createTimer(ros::Duration(1.0), &icp_transform::transform,this);
}

//pointcloud取得関数
void icp_transform::cb_catch_cloud2_first(const sensor_msgs::PointCloud2::ConstPtr& cloud2){
  cloud2_first=*cloud2;
  pcl::fromROSMsg(cloud2_first,pcl_first);

  //ROS_INFO("first");
  //pub_pcl_first.publish(pcl_first);
}

void icp_transform::cb_catch_cloud2_second(const sensor_msgs::PointCloud2::ConstPtr& cloud2){
  cloud2_second=*cloud2;
  pcl::fromROSMsg(cloud2_second,pcl_second);
}

//表示関数
void print4x4Matrix (const Eigen::Matrix4d & matrix)
{
  printf ("Rotation matrix :\n");
  printf ("    | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));
  printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));
  printf ("    | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));
  printf ("Translation vector :\n");
  printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
}


void icp_transform::transform(const ros::TimerEvent&){
  //cloud_firstとcloud_secondをICPアルゴズムにより変換matrixを求める。
  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

  //pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_in (&pcl_first);

  //boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > cloud_in (&pcl_first);
  //boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > cloud_out (&pcl_second);

  //icp.setInputSource(cloud_in);
  //icp.setInputTarget(cloud_out);
  icp.setInputSource(pcl_first.makeShared());
  icp.setInputTarget(pcl_second.makeShared());

  pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);

  //変換matrixを表示する
  Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();
  transformation_matrix = icp.getFinalTransformation ().cast<double>();
  print4x4Matrix (transformation_matrix);
}


//
int main(int argc, char** argv)
{
    ros::init(argc, argv, "icp_transform");

    icp_transform icp_transform;

    ros::spin();

    return 0;
}

参考にしたのは,
qiita.com

ros-robot.blogspot.jp

http://derivecv.tumblr.com/post/12364144145
derivecv.tumblr.com

www.slideshare.net


大体のところは,上の参考文献を当たればわかったのですが,唯一分かりにくくて困ったのは
pcl_first.makeShared()
この部分です

自分は,
URGのScanデータ→Pointcloud→Pointcloud2→2つのPointcloud2を同じ座標軸に変換→pcl::PointCloud→pcl::PointCloud::ConstPt
という長い手順でPCLライブラリに適用できるような形にしています
本来なら,PointCloudの形にして自分でICPアルゴリズムが書けるのがベターなのはわかっていますができないので,このような手順をとっています
問題だったのは,
pcl::PointCloud→pcl::PointCloud::ConstPt
の変換です

ICPアルゴリズムに限らず,PCLライブラリを使うにはスマートポインタの形にすることが大半だそうです
で,この変換に苦労しました
コメントアウトが多いところがあるのですが,その部分がスマートポインタの形にするのに苦労したところです

どうすればいいかというと
pcl_first.makeShared()
このように,pcl::PointCloudのクラス?の関数を実行するだけで,スマートポインタの形に出来ました

やっぱり,C++のリファレンスとかクラスの概念が全く分かっていないのでめっちゃ詰まりましたが,できたので良かったです

2017/12/10(休日)

明日からまた研究かぁ・・・・

 

今日は,台所の掃除してからカフェで本を読んでいました

onsaya.com

奉還町の店舗に行きました

この店舗,2階にいい席があってふかふかのソファに座れる上に店員さんも注文の品をもって来る以外に来ないのでとても落ち着けます

ですが,今日はランチの時間に行ってしまって,1階のカウンターで飲んでいました

 

人気店なので仕方なかったのですが少し落ち着けない状態で本を読んでいました

 

読んだのは昨日買ったばかりの

「僕はロボットごしの君に恋をする」を読みました

感想は,なんかよくある感じのオチだった,といえば少し偉そうですがそんな感じです

 

明日からは冬シンポのスライド作りが待っています

2017/12/9(原稿終わり)

冬シンポの原稿が終わった

 

やっと冬シンポの原稿が終わりました

実験して,ROSからデータ抜き出してちゃんとそれっぽくなりました

実験結果は思ってた通りの結果が出たのでまあよかったのかなと思います

 

なので,今日は久しぶりの休みでした

朝起きてからトイレ掃除と風呂掃除をして,学食で昼飯食べて,部室でクロスバイク洗って,イオンで本買った後に,映画見て帰った来ました

全く研究室に行かない日でした

めっちゃ楽しかった

 

イオンで本を買ったのですが,

「僕はロボットごしの君に恋をする」を買いました

この前,学校の生協の還元祭のガラガラで図書カード1000円が当たったので何か買いたいなぁと思っていたので衝動買いしました

 

映画は,

GODZILLA 怪獣惑星」

を見に行きました

友達から誕プレで映画観賞券を2枚もらっていて今まで使っていなかったので,思い出していきました

(2枚あるってことは彼女作って行けということなんでしょうね)

あと,一枚余ってるけど次は何の映画行こうかなぁ

 

明日はカフェで買った本を読もう