ROS Industrial (Kinetic) 演習トレーニング

PC のセットアップ

PC のセットアップ

ROS Industrial トレーニング教材を利用するには 2つの方法があります． 推奨方法 は事前に設定された仮想マシンを利用することです． もう1つの方法は必要なソフトウェアを備えた ネイティブの Ubuntu マシンをインストールすることです． 仮想マシンを用いた方法は，はるかに簡単な選択肢で トレーニング中のビルドエラーを最小限に抑えますが， 特定のハードウェア，特に USB（ Kinect のようなデバイスなど ） に接続するには機能的な限界があります． 本トレーニングコースの知覚機能トレーニングにおいて USB 接続を必要としなくて済むように .bag ファイルを用意しています．

仮想マシンの設定（推奨）

仮想マシン（VM）はトレーニング教材を利用する最も便利な方法です．

1. VirtualBox をダウンロード

2. ROS Kinetic Industrial Training VM イメージのダウンロード

3. VirtualBox への VM イメージのインポート

4. 仮想マシンの起動

5. ユーザ名: ros-industrial ，パスワード: rosindustrial（スペース・ハイフンなし）でログイン

6. ターミナルを開いて最新の状態に更新

   cd ~/industrial_training
   git checkout kinetic
   git pull
   ./.check_training_config.bash
   

VirtualBox で制限される機能

VirtualBox はハードウェア機能（VMの制限による）と パッケージのインストール（スペースを節約するため）の 2点で制限があります． USB の制約のため Kinect を用いたデモはできません．

仮想マシン一般の問題

最近のほとんどのシステムでは VirtualBox などの仮想マシンはそのまま利用できます． 次は他の人が遭遇した問題とその解決法です．

• 仮想化を有効にする必要があります - 古いシステムでは デフォルトで仮想化が有効になっていません． BIOS で仮想化を有効にする必要があります． http://www.sysprobs.com/disable-enable-virtualization-technology-bios を参考にしてください．

ネイティブ Linux PC の設定（非推奨）

インストール シェルスクリプト は Ubuntu Linux 16.04（Xenial Xerus）LTS で動作するようになっています． このスクリプトは本トレーニングに使用する環境に必要な ROS およびその他のパッケージをインストールします．

この手順の後（もしくは既に ROS 環境がある場合） トレーニング教材リポジトリをホームディレクトリに複製します．

git clone -b kinetic https://github.com/ros-industrial/industrial_training.git ~/industrial_training

設定の確認

以下は適切なパッケージがインストールされ industrial_training git リポジトリが最新であること を確認するための簡単なチェック方法です． ターミナルで次のコマンドを実行してください．

~/industrial_training/.check_training_config.bash

準備

C++

Linux の基礎

スライド

Ubuntu の GUI 操作

オペレーティング・システム Ubuntu の グラフィカル・ユーザ・インタフェース（GUI）に慣れるようにします．

タスク 0: プレゼンテーション・スライド

プレゼンテーション・スライド の PDF ファイルがこのトレーニングには添付されています．

タスク 1: Ubuntu デスクトップに慣れる

ログイン画面でパスワード入力欄に rosindustrial を入力して Enter キーを押してください． ログインできた場合には 次のような画面が見えているはずです．

デスクトップ上のいくつかの項目について見ていきます．

1. 画面の右上にある歯車アイコンは， ユーザのログアウトやコンピュータのシャットダウン， システム設定へのアクセスなどを行うための メニューを表示します．
2. 左側のバーには， 実行中の「お気に入り」アプリケーションや 接続された USB ドライブなどが表示されます．
3. 最上部のアイコンは， 全てのアプリケーションとファイルにアクセスするために使用されます． これについては後で詳しく見ていきます．
   1. 続くアイコンは， 現在実行中のアプリケーションか 「ピン留め」されているアプリケーションです．（後で詳述）
   2. USB ドライブのようなリムーバブル・ドライブは アプリケーションのアイコンの後にあります．
   3. ランチャーバーがあまりにも多くになった場合， クリックして上下にドラッグすると 非表示のアプリケーションを見ることができます．
   4. ランチャーのアイコンを再編成するには 「飛び出す」までアイコンをクリックしたままにして 目的の場所に移動します．

タスク 2: アプリケーションを開く・調べる

ランチャーのファイリング・キャビネットの形をしたアイコンをクリックします． ウィンドウが表示され，デスクトップは次のようになります．

注記事項:

1. 通常は 閉じる・最小化・最大化 のボタンが ウィンドウのタイトルバーの左側にあります．
2. ウィンドウのメニューは， 画面の上部にあるメニューバーに表示されます． これは Mac の場合と同じです． ただしメニューはメニューバーの上に マウスを置いたときのみ表示されます．
3. フォルダアイコンの左右に三角形があることに注目してください． 左の三角はこのアプリケーションで いくつのウィンドウが開いているかを示し， 右は現在どのアプリケーションが最前面にあるか， または「フォーカスがある」ことを示しています． アプリケーションが開いているときにアイコンをクリックすると， 次の2つのうちのいずれかが実行されます．
   • 開いているウィンドウが1つだけの場合， そのウィンドウにフォーカスが移ります．
   • 複数のウィンドウが開いている場合は， 2回目のクリックをすると， すべてのウィンドウが最前面に表示されるため， どのウィンドウを表示するかを選択できます． （下図参照）

タスク 3: アプリケーションを開始する・ランチャーバーへのピン留め

ランチャーボタン（左上）をクリックし， 検索ボックスに「 gedit 」と入力します． 「Text Editor」アプリケーション （これがまさに gedit です）が表示されます． （下図参照）

アプリケーションをクリックします． テキストエディタのウィンドウが画面に表示されて テキストエディタ・アイコンが左側のランチャーバーに表示されます． （下記参照）

1. ランチャーアイコンを右クリックして "Lock to Launcher" を選択してください．
2. gedit ウィンドウを閉じてください． 閉じてもランチャーアイコンがまだ残っているはずです．
3. gedit ランチャーアイコンをクリックしてください． 新しい gedit ウィンドウが表示されるはずです．

Linux のファイルシステム

Ubuntu GUI を使用して Linux ファイルシステム内のファイルをナビゲートして移動する方法と， Linux ファイル属性の基本について学びます．

ファイルブラウザの利用

1. 前の演習 で開いたファイルブラウザ・アプリケーションを開きます． 次のようなウィンドウが表示されます． メインウィンドウ上部のアイコンとテキストはファイルシステム内の現在の位置を示します．

   

2. 上部のアイコンはファイルブラウザの「ロケーションバー」を構成しています． ロケーションバーは GUI でのナビゲートに非常に便利ですが ウィンドウの正確な位置を表示していません． Ctrl + L を押すと位置を表示できます． ロケーションバーが次のように変わっているはずです．

   

3. デフォルトではユーザのホームフォルダにファイルブラウザが開きます． このフォルダは通常 /home/ で， ROS-Industrial トレーニング・コンピュータでは /home/ros-industrial です． このフォルダはユーザが完全にアクセスできる唯一のフォルダです． これはセキュリティ上の理由によるものです．

4. デフォルトではファイルブラウザは隠しファイル（ . 文字で始まるファイル） またはバックアップ・ファイル（ ~ 文字で終わる）を表示しません． これらのファイルを表示するには [View] メニューをクリックし [Show Hidden Files] を選択する，または Ctrl + H キーを押してください． すべての隠しファイルが表示されます． これらのファイルを再び隠すオプションをオフにします．

5. 2つの隠しディレクトリは表示されません． 現在のフォルダーを表す特別な . フォルダーと， 現在のフォルダーを含むフォルダーを表す .. フォルダの2つです． これらは 次の演習 で重要になります．

6. ウィンドウの左側にはリムーバブルデバイスや他のハードドライブ， ブックマークなどへのクイックリンクがあります． 「Computer」ショートカットリンクをクリックします． これでファイルシステムの "root" の / フォルダに移動します． コンピュータ上のすべてのファイルはこのフォルダの下のサブフォルダにあります．

7. opt フォルダ，次に ros フォルダをダブルクリックします． ここは全ての ROS ソフトウェアが存在する場所です． 各バージョンはそれぞれのフォルダに格納されています． そこに kinetic フォルダがあるはずです． そのフォルダをダブルクリックします． setup.bash ファイルは ターミナル演習 で使用され， ターミナルを ROS 用に設定します． プログラムやデータなどは bin と share フォルダにあります． 一般的にこれらのファイルを直接変更する必要はありませんが， それらがどこにあるのかを知っていると良いです．

フォルダ・ファイルの変更

ファイルのコピー・移動・削除

1. 新しいフォルダ・ファイルの作成

   1. <Home>/ex0.3 フォルダを作成します．

      今後はここで演習を行います．

      • ファイルブラウザ内で左のサイドバーにある "Home" ショートカットをクリックします．
      • ファイルブラウザのメインパネル内で右クリックして "New Folder" を選択します．
      • フォルダ名を "ex0.3" にして Enter キーを押します．

   2. 新しく作成した ex0.3 フォルダ内に test.txt というファイルを作成します．

      • ex0.3 フォルダをダブルクリックします． ファイルブラウザのヘッダがどのように変更されて 現在のフォルダを表示するのかに着目してください．
      • ファイルブラウザのメインパネル内で右クリックして "New Document" と， 次に "Empty Document" を選択してください．
      • ファイル名を "test.txt" としてリターンを押します．

2. ファイルのコピー

   1. 次のいずれかの方法でファイルをコピーしてください．
      • test.txt ファイルをクリック&ホールドし， Control キーを押したまま， どこか他のフォルダにドラッグして放します．
      • ファイルをクリックし "Edit" メニューの "Copy" をクリックしてから "Edit" メニューから "Paste" をクリックします．
        • 注: メニューを表示するには画面上部のバーの上にマウスを置いてください．
   2. 次のいずれかの方法で，コピーしたファイルの名前を copy.txt に変更してください．
      • コピーしたファイルを右クリックして "Rename..." を選択し copy.txt と入力する．
      • ファイルをクリックし F2 キーを押して copy.txt と入力する．
   3. 次のいずれかの方法で new フォルダを作成してください．
      • ファイルブラウザ・ウィンドウの空いている領域を右クリックし "New Folder" を選択して 名前を new にする．
      • "File" メニューから "New Folder" を選択し new という名前をつける．
   4. ファイルを new フォルダにドラッグして copy.txt を new フォルダに移動します．
   5. Control キーを押しながら ファイルを新しいフォルダにドラッグして test.txt ファイルをコピーします．
   6. new フォルダに移動し， test.txt ファイルをクリックした後， Delete キーを押してファイルを削除します．

Linux のターミナル

Linux のターミナル操作に慣れるようにします．

ターミナルの起動

1. ターミナルを開くには下記のアイコンをクリックします．

   

2. 2つ目のターミナルを次のいずれかの方法で開きます．

   • ターミナル上で右クリックして "Open Terminal" を選択する．
   • "File" メニューから "Open Terminal" を選択する．

3. ターミナルウィンドウが選択されているときに "Ctrl+Shift+T" を押すと， 同じウィンドウ内に2つ目のターミナルが開きます．

4. 2つ目のターミナルのタブを次のいずれかの方法で閉じてください．

   • ターミナルタブにある小さな 'x' をクリックする． （メインのターミナルウィンドウの 'x' ではありません）
   • exit とタイプして Enter を押す．

5. ウィンドウは次のような1行が表示されています．

   ros-industrial@ros-i-kinetic-vm:~$

6. これは「プロンプト」と呼ばれるものでコマンドを入力する場所です． このプロンプトにはデフォルトでは3つの情報が表示されています．

   1. ros-industrial は現在のユーザのログイン名です．
   2. ros-i-kinetic-vm はコンピュータのホスト名です．
   3. ~ は現在のターミナルがいるディレクトリです．（後で詳述）

7. exit を入力するか ウィンドウタイトルバーにある赤い 'x' をクリックして ターミナルウィンドウを閉じます．

ディレクトリの移動とファイルのリスト表示

環境の準備

1. ファイルブラウザでホームフォルダを開いてください．
2. 前の演習で作成した ex0.3 フォルダを ダブルクリックしてください．
   • このフォルダを利用してターミナルの さまざまなファイル操作を説明します．
3. 現在のディレクトリでターミナルを開くために， ファイルブラウザのメインウィンドウで右クリックして "Open in Terminal" を選択してください．
4. ターミナルウィンドウで次のコマンドを入力して この後の演習で使ういくつかのサンプルファイルを作成します．
   • cp -a ~/industrial_training/exercises/0.3/. .

ls コマンド

1. ターミナルで ls と入力してください．
   • test.txt と new がリスト表示されるはずです． （ new が表示されない場合は 前回の演習 を完遂してください．）
   • new のようなディレクトリは青色になっています．
   • ファイル sample_job は緑色になっています． これは「実行」のためのビットセットを有しているという印で， コマンドとして実行可能であることを意味しています．
2. ls *.txt と入力してください． test.txt だけが表示されるはずです．
3. ls -l とターミナルに入力してください．
   • -lオプションを追加すると，1行に1つのエントリが表示され， ディレクトリ内の各エントリに関する追加情報が表示されます．
   • 最初の10文字はファイルの種類とアクセス許可を示します．
   • エントリがディレクトリの場合，最初の文字は d です．
   • 次の9文字はファイルのパーミション（許可）ビットです．
   • 3番目と4番目のフィールドはそれぞれ所有するユーザーとグループです．
   • 2番目から最後までのフィールドは、ファイルが最後に更新された時刻です．
   • ファイルがシンボリックリンクの場合， リンクのファイル名の後ろにリンクのターゲットファイルがリストされます．
4. ls -a とターミナルに入力してください．
   • ここでは隠しファイルが1つ追加で表示されます．
5. ls -a -l もしくは ls -al とコマンド入力してください．
   • これで hidden_link.txt ファイルが .hidden_text_file.txt を指していることがわかります．

pwd と cd コマンド

1. pwd とターミナルに入力してください．
   • 現在の作業ディレクトリの完全なパスを表示します．
2. cd new とターミナルに入力してください．
   • プロンプトが変わって ros-industrial@ros-i-kinetic-vm:~/ex0.3/new$ と表示されるはずです．
   • ここで pwd を入力すると /home/ros-industrial/ex0.3/new ディレクトリにいることを表示します．
3. cd .. とターミナルに入力してください．
   • 前の演習 で .. が親フォルダであることを示しました．
   • プロンプトは現在の作業ディレクトリが /home/ros-industrial/ex0.3 であることを示すはずです．
4. cd /bin と入力してから ls を実行してください．
   • このフォルダには 最も基本的な Linux コマンドのリストが含まれています．
     • pwd と ls はこのフォルダにあります．
5. cd ~/ex0.3 と入力して 本演習の作業ディレクトリに戻ります．
   • Linux では ~ 文字を ホームディレクトリの省略表現として使用します．
   • コマンドラインのコマンドで ファイルとパスを参照する便利な方法です．
   • 本演習で沢山この ~ を入力しますので是非憶えてください．

本節のコマンドの全オプションを知りたい場合は， コマンドラインに man <command> と入力します． （ <command> は欲しい情報があるコマンド名です．） これによりコマンドの組込みドキュメントが表示されます． スクロールするには矢印と Page Up/Down キーを使い， 終了するには q を使います．

ファイルの変更

mv コマンド

1. mv test.txt test2.txt と入力してから ls を行ってください．
   • test2.txt にファイル名が変更されているはずです．
     ここでは mv でファイル名を変更する方法を見ました．
2. mv test2.txt new と入力してから ls を行ってください．
   • ファイルがフォルダ内にないことがわかると思います．
3. cd new と入力してから ls を行ってください．
   • フォルダ内にファイル test2.txt があるはずです．
     ここでは mv でファイルを移動する方法を見ました．
4. mv test2.txt ../test.txt と入力してから ls を行ってください．
   • test2.txt はそこにはないはずです．
5. cd .. と入力してから ls を行ってください．
   • test.txt が再び表示されているはずです．
     ここでは mv で ファイル名の変更と移動を同時に行う方法を見ました．

cp コマンド

1. cp test.txt new/test2.txt と入力してから ls new を行ってください．
   • test2.txt が new フォルダ内にあるはずです．
2. cp test.txt "test copy.txt" と入力してから ls -l を行ってください．
   • test.txt が test copy.txt に コピーされているはずです．
     引用符 " はスペースや特殊文字が ファイル名に含まれる場合に必要となります．

rm コマンド

1. rm "test copy.txt" を入力してから ls -l を行ってください．
   • test copy.txt が無くなっているはずです．

mkdir コマンド

1. mkdir new2 を入力してから ls を行ってください．
   • new2 というフォルダができているはずです．

-i フラグを cp や mv および rm コマンドとともに使用すると， ファイルの上書きまたは削除を行うときに注意喚起されます．

ジョブの管理

ジョブの停止

1. ./sample_job を実行してください．
   • プログラムが開始します．
2. Control+C を押してください．
   • プログラムから抜けます．
3. ./sample_job sigterm を実行してください．
   • プログラムが開始します．
4. Control+C を押してください．
   • 今回はプログラムは停止しません．

制御不能ジョブの停止

1. 新たにターミナルのウィンドウを開きます．
2. ps ax と入力してください．
3. ターミナルをスクロールアップして python ./sample_job sigterm を見つけてください．
   • これは先程の1つ目のウィンドウで実行したジョブです．
   • テーブルの最初のフィールドはプロセス ID です． （ man ps で他のフィールドについても学習してください．）
4. ps ax | grep sample と入力してください．
   • より少ない行が表示されているはずです．
   • これは特定のプロセスを見つけたい場合に便利です．
   • 注: これはパイピングという応用技術で， あるプログラムの出力が次のプログラムの入力に渡されます． これは本演習の目的の範囲を超えていますが， ターミナルをより広範に使用するようでしたら便利な機能です．
5. kill <id> と入力してください． <id> は ps ax で調べたジョブ番号です．
6. 1つ目のターミナルで ./sample_job sigterm sigkill と入力してください．
   • プログラムが開始されます．
7. 2つ目のターミナルでプロセス ID を得るために ps ax | grep sample と入力してください．
8. kill <id> と入力してください．
   • 今回はプロセスが終了しません．
9. kill -SIGKILL <id> と入力してください．
   • 今回はプロセスが終了するはずです．

プロセスとメモリの使用状況の表示

1. ターミナルで top と入力してください．
   • テーブルが表示され，1秒につき1回更新されます． システムの全プロセスと 全体の CPU と メモリの使用状況が表示されます．
2. Shift+P キーを押してください．
   • これは CPU の利用割合の順番で並び替えを行います．
     どのプロセスが最も CPU を使用しているかを判断するのに利用できます．
3. Shift+M キーを押してください．
   • これはメモリの利用割合の順番で並び替えを行います．
     どのプロセスが最もメモリを使用しているかを判断するのに利用できます．
4. q か Ctrl+C でプログラムを終了します．

テキストの編集とその他の GUI コマンド

1. gedit test.txt と入力してください．
   • 新しくテキストエディタのウィンドウが開いて test.txt が読み込まれます．
   • ウィンドウを閉じるまで ターミナルのプロンプトは戻ってきません．
2. プロンプトが戻ってくるようにする方法は 2つありますので両方とも試してみてください．
   1. プログラム開始と同時にプロンプトを回復させる:
   2. gedit test.txt & と入力してください．
      • & の文字はコマンドの実行を バックグラウンドで行うことをターミナルに指定します． つまりプロンプトがすぐに戻ってくることを意味します．
   3. gedit ウィンドウを閉じて， ターミナルで ls と入力してください．
      • ターミナルにはファイルリストが表示されるだけでなく gedit が完了したことも通知されます．
   4. 既に開始しているプログラムをバックグラウンド実行に変更する:
   5. gedit test.txt と入力してください．
      • ウィンドウが開いてターミナルでは プロンプトが待機状態にはなっていないはずです．
   6. ターミナルのウィンドウ内で Ctrl+Z を押してください．
      • ターミナルには gedit が終了した旨が表示されて， プロンプトが現れます．
   7. gedit ウィンドウを使ってみてください．
      • 一時停止状態のためウィンドウは動作しません．
   8. ターミナルで bg と入力してください．
      • gedit ウィンドウは動作をします．
   9. gedit ウィンドウを閉じて， ターミナルで ls と入力してください．
      • 前と同じようにターミナルには gedit が完了したことが通知されます．

ルートとしてのコマンド実行

1. ターミナルで ls -a /root と入力してください．
   • 端末は /root フォルダを読むことができないことを示します．
   • 普通のユーザーとしては実行できないコマンド を実行する必要がある場合が多くあり， その時は「スーパーユーザー」として コマンドを実行する必要があります．
2. 前のコマンドを root として実行するには， コマンドの先頭に sudo を追加します．
   • この場合 sudo ls -a /root と入力してください．
   • 実行において端末はパスワードを要求します． （本例では rosindustrial ）
   • パスワードを入力すると， /root ディレクトリの内容が表示されます．

警告:sudoは 実行に際して指示の健全性のチェックを行わない 強力なツールですので 使用する際に 特に注意 してください．

基礎編

セッション 1 - ROS の概念と基礎

スライド

ROS のセットアップ

ターミナルから ROS をセットアップし，roscore を起動します．

モチベーション

ROS でプログラミングを開始するにあたり， 新しいマシンに ROS をインストールする方法や， インストールしたものが適切に機能すること を確認する方法も知っておいた方が良いでしょう． 本演習ではインストールされている ROS システムの 簡単なチェックを行います． 仮想マシンから作業している場合は ROS がすでにインストールされているため インストール手順をスキップできます．

リファレンス

• Installing and Configuring Your ROS Environment
• ROS環境のインストールとセットアップ

追加情報とリソース

• Installation Instructions
• Navigating ROS

Scan-N-Plan アプリケーション: 演習問題

ROS のインストールが正常にされているか確かめるためのテストを行います．

• Scan-N-Plan アプリケーション: スキャンと動作計画を行うアプリケーション

Scan-N-Plan アプリケーション: ガイダンス

~/.bashrc の設定

1. ROS パッケージを探したり使用したりする際に 問題が発生した場合は， 環境が適切に設定されていることを確認してください． ROS_ROOT や ROS_PACKAGE_PATH などの 環境変数がどのような設定になっているのか を確認すると良いです．

   printenv | grep ROS
   

2. 環境設定が適切でなかった場合は _setup.*sh_ ファイルを source する必要があります．

   source /opt/ros/kinetic/setup.bash
   

3. ROS をインストールした素の状態では ROS コマンドにアクセスするために開く 新規のシェル（ターミナル）でこの コマンドを実行する必要があります． 典型的な ROS インストールにおいては 設定手順の1つとして，このコマンドを ~/.bashrc ファイルの最後に追加して， ターミナルウィンドウを開くたびに 自動で実行されるようにします． ~/.bashrc ファイルが 既に ROS-kinetic の setup.bash を実行するように 設定されているかを確認してください．

   tail ~/.bashrc
   

このプロセスにより 複数の ROS ディストリビューション （例えば indigo と kinetic）を 同じコンピュータにインストールし， 各ディストリビューションの setup.bash ファイルをソースして それらを切り替えることができます．

roscore の起動

1. roscore は ROS ベースのシステムに必要な ノードとプログラムの集合です． ROS ノードが通信できるようにするには roscore を実行しておく必要があります． roscore コマンドを使用して起動します。

   roscore
   

   roscore が次を起動します．

   • ROS マスター
   • ROS パラメータサーバ
   • ROS ロギングノード

   started core service [/rosout] で終わる表示がされているはずです． roscore: command not found と表示されたら， ROS の環境が設定されていません． 詳しくは 演習 5.1 の .bashrc のセットアップを参照してください．

2. ロギングノードを見るには 新しくターミナルを開いて 次のコマンドを実行してください．

   rosnode list
   

   ロギングノードは /rosout です．

3. 1つ目のターミナルで Ctrl+C を押して roscore を停止します． Ctrl+C は ほとんどの ROS コマンド を停止するために使用される一般的な方法です．

Catkin ワークスペースの作成

ROS の catkin ワークスペースを作成します．

モチベーション

ROS プロジェクトは作業領域の作成から始まります． このワークスペースには プロジェクトに関連するすべてのものを配置します． 本演習では Scan-N-Planアプリケーションの コンポーネントを作成するワークスペースを作成します．

リファレンス

• ワークスペースの作成手順:
  • Creating a Catkin Workspace
  • catkin の workspace を作る

注: ros.org の現在の多くの例は、 古いスタイルの catkin_init_workspace コマンド を使用しています．似てはいますが 本演習で使用する catkin_tools コマンドを そのまま置き換えることはできません．

追加情報とリソース

• Catkin ワークスペスの利用:
  • Using a Workspace
  • workspace で catkin パッケージをビルドして使う

Scan-N-Plan アプリケーション: 演習問題

まず ROS が正しくインストールされている必要があります． そして最初のステップとして 我々のアプリケーション固有の設定する必要があります． 目標はアプリケーションとその補足ソフトウェアのために ワークスペース（ catkin ワークスペース）を作成することです．

Scan-N-Plan アプリケーション: ガイダンス

catkin ワークスペースの作成

1. ワークスペースの大本（ルート）となる ディレクトリを作成します． （ ここでは catkin_ws とします．)

   cd ~/
   mkdir --parents catkin_ws/src
   cd catkin_ws
   

2. catkin ワークスペースを初期化します．

   catkin init
   

   • "Workspace configuration valid" と表示されて catkin ワークスペースが正常に作成されたことが表示されます． src ディレクトリを作成するのを忘れた場合や ワークスペースのルートから catkin init を実行しなかった場合（どちらもよくある間違いです）， WARNING: Source space does not yet exist というエラーメッセージが表示されます．

3. ワークスペースをビルドします．

   次のコマンドはワークスペースの ルートディレクトリ（つまり catkin_ws ）下であれば どこでも実行することができます．

   catkin build
   ls
   

   • catkin_ws ディレクトリに 追加のディレクトリ（ build, devel, logs ） が含まれるようになります．

4. これらの新しいディレクトリは （ 手動で，または catkin clean を使って ） いつでも安全に削除できます．

   catkin は src ディレクトリ内の ファイルを決して変更しません． catkin build を再実行して build / devel / logs ディレクトリを再生成します．

   catkin clean
   ls
   catkin build
   ls
   

5. ROS から作業領域が見えるようにします．

   devel ディレクトリにある セットアップファイルを実行してください．

   source devel/setup.bash
   

   • これは新しくターミナルを立ち上げるたびに 実行する必要があります．
   • 入力を省力するためには このコマンドを 〜/.bashrc ファイルに追加して， 新しくターミナルを起動するたびに自動的に実行されるようにします．
     1. gedit ~/.bashrc
     2. ファイルの最後に次の1行を追加: source ~/catkin_ws/devel/setup.bash
     3. 保存してエディタを閉じる

パッケージのインストール

モチベーション

ROS の最もクールで最も便利な機能の多くは 既に ROS コミュニティのどこかに存在しています． 多くの場合，安定版のリソースがダウンロードしやすい debian パッケージとして存在しています．
テストされていないか，より「最先端」であり， 安定的なリリースには達していないリソースもあります． これらのリソースの多くはリポジトリ （通常は GitHub に置かれています） からダウンロードしてアクセスすることができます． これらの git パッケージを入手するには debian パッケージよりもいくつかのステップが多く必要となります． 本演習では両方のタイプのパッケージにアクセスし， それらをシステムにインストールします．

リファレンス

• apt-get usage

追加情報とリソース

• Ubuntu apt-get How To
• Git Get Repo
• Git Clone Documentation

Scan-N-Plan アプリケーション: 演習問題

まず ROS が正しくインストールされている必要があります． そして ROS に存在するいくつかのパッケージを 本プログラム内で使用したいと考えています． 安定版のパッケージがあり， そのダウンロード可能な debian パッケージが見つかったとします． また，興味深い安定版の git パッケージも見つけたとします． ROS の世界を訪れて これらのパッケージをダウンロードしてください！

1. 使用したい特定のメッセージタイプがあります． calibration_msgs という 安定版 ROS パッケージがあります．
2. AR タグを使用したいのですが， テスト目的のために ノードが似たような情報を発行するものが欲しいです． : fake_ar_publisher

パッケージ／ワークスペース内の これらの両パッケージのリソースにアクセスすることが目標です．

1. calibration_msgs（ apt-get を使用 ）
2. fake_ar_publisher ( git から取得 )

Scan-N-Plan アプリケーション: ガイダンス

apt リポジトリからのパッケージのインストール

1. ターミナルのウィンドウを開いて， roscd calibration_msgs と入力してください．

   roscd calibration_msgs
   

   • このコマンドは作業ディレクトリを ROS calibration_msgs パッケージの ディレクトリに変更します．
   • 次のようなエラーメッセージが表示されるはずです． **No such package/stack 'calibration_msgs**'
   • このパッケージはシステム内にインストールされていないので インストールします．

2. apt install ros-kinetic-calibration-msgs と入力してください．

   apt install ros-kinetic-calibration-msgs
   

   • プログラムはパッケージをインストールできないと表示して ルート（root）としてプログラムを実行する必要があることを提示します．
   • パッケージ名を入力する際に TAB キーを押してください．
     • インストール可能であれば システムは自動的にパッケージ名を完成させようとします．
     • TAB キーを常用すると多くの定型コマンドの入力を高速化できます．

3. sudo apt install ros-kinetic-calibration-msgs と入力してください．

   sudo apt install ros-kinetic-calibration-msgs
   

   • sudo コマンドを使用して "root"（管理者）権限を持つコマンド を実行することに注意してください．
   • パスワードを入力し， （尋ねられたら）プログラムのインストールを確認します．

4. roscd calibration_msgs と再び入力してください．

   roscd calibration_msgs
   

   • 今度は作業ディレクトリが /opt/ros/kinetic/share/calibration_msgs に変更されたはずです．

5. sudo apt remove ros-kinetic-calibration-msgs と入力して パッケージを削除します．

   sudo apt remove ros-kinetic-calibration-msgs
   

   • 安心してください． 今後の演習にはこのパッケージは必要ないため 削除しても大丈夫です．

6. cd ~ と入力して ホームディレクトリに戻ります．

   cd ~
   

ソースからダウンロードしてパッケージをビルドする

1. 目的のパッケージのソースリポジトリを特定します．

   1. GitHub を開きます．
   2. fake_ar_publisher を検索します．
   3. このリポジトリをクリックして 右の方にある Clone or download をクリックして， そのパネル内から URL をクリップボードにコピーします．

2. fake_ar_publisher をクローンします．

   fake_ar_publisher のリポジトリ から catkin ワークスペース内の src ディレクトリにクローンします．

   cd ~/catkin_ws/src
   git clone https://github.com/jmeyer1292/fake_ar_publisher.git
   

   • Ctrl-Shift-V を押してターミナルに貼り付けるか， マウスの右クリックから貼り付けを選択します．
   • Git コマンドは本課題の範囲外ですが， GitHub に良いチュートリアル があります．

3. catkin build を使って 新しいパッケージをビルドします．

ビルドコマンドは catkin ワークスペースのどこからでも実行できます．

1. ビルドが終了したら "re-source setup files to use them" との指示が出ます．

   • 前回の演習で 〜/.bashrc ファイルに行を追加して 新しくターミナルを起動するたびに catkin 設定ファイルを 自動的に再実行するようにしました．

   • ほとんどの開発の場面ではこれで十分ですが， 例えば新しいパッケージを追加するような場合など， 現在のターミナルで source コマンド を再実行する必要が生じることがあります．

     source ~/catkin_ws/devel/setup.bash
     

2. ビルドが終了したら build と devel ディレクトリを調べて 作成されたファイルを確認してください．

3. rospack find fake_ar_publisher を実行して 新しいパッケージが ROS から見えていることを確認します．

   rospack find fake_ar_publisher
   

   • このコマンドは ROS ワークスペースに関連する問題の トラブルシューティングに役立ちます．
   • ROS がパッケージを見つけることができない場合は， ワークスペースの再ビルドを行い， その後ワークスペースの setup.bash ファイル を再実行してみてください．

パッケージとノードの作成

ROS のパッケージとノードを作成します．

モチベーション

ROS の通信は， ノードと呼ばれる複数の実行可能ファイルが環境内で実行され， それらがさまざまな方法で相互に通信することが基本となっています． これらのノードはパッケージと呼ばれる構造内に存在します． 本演習では新たに作成したパッケージ内にノードを作成します．

リファレンス

• Creating a ROS Package
• ROSパッケージを作る

追加情報とリソース

• Building a ROS Package
• Understanding ROS Nodes

• ROSのパッケージをビルドする
• ROSのノードを理解する

Scan-N-Plan アプリケーション: 演習問題

これまでに ROS をインストールし， ワークスペースを作成し， さらにビルドを何回か行いました． ここでは自分たちが行いたいことをするために 独自のパッケージと独自のノードを作成します．

目標は ROS ノードを作成することです．

1. まず catkin ワークスペース内に パッケージを作成する必要があります．
2. 次にそこに独自のノードを記述します．

Scan-N-Plan アプリケーション: ガイダンス

パッケージの作成

1. catkin ワークスペースの src ディレクトリに cd します．

   注: src ディレクトリ内に 全てのパッケージが作成されることを 憶えいておいてください．

   cd ~/catkin_ws/src
   

2. ROS のコマンドを利用して roscpp に依存関係を有する myworkcell_core という パッケージを作成します．

   catkin create pkg myworkcell_core --catkin-deps roscpp
   

   ドキュメント catkin_tools を参照してこのコマンド理解を深めてください．

   • 本コマンドは新しいパッケージのための 新しいディレクトリと 必要なファイルを作成します．
   • 最初の引数は ROS パッケージの名前です．
   • --catkin-deps を用いて 新しいパッケージから依存関係にある パッケージを設定します．

3. myworkcell_core という名前の フォルダができているはずです． そのフォルダに移動して package.xml ファイルを編集します． description や author など の記述を編集します．

   cd myworkcell_core
   gedit package.xml
   

   パッケージ作成時に 依存関係を追加することを忘れた場合は， 後から package.xml ファイルに 依存関係を追加することができます．

ストップ！ この演習を続ける前にいくつかの講義スライドをもう一度見ていきます．

ノードの作成

1. 本パッケージのフォルダで gedit を使用して CMakeLists.txt ファイルを編集します．

   例として書かれているルールを参考にしながら， 実行可能ファイル（ add_executable ）に vision_node という名前のノードと vision_node.cpp という名前の ソースファイルを追加します． また CMakeLists.txt 内で 新しく作成するノード vision_node が catkin ライブラリにリンクされている （ 'target_link_libraries' ）こと を確認してください．

   add_compile_options(-std=c++11)
   add_executable(vision_node src/vision_node.cpp)
   target_link_libraries(vision_node ${catkin_LIBRARIES})
   

   これらの行は CMakeLists.txt の中の どこに記述されていても大丈夫ですが， 一般的には次のようにしています．

   • 上の add_compile_options の 既存のテンプレートのコメントを外します（ project() の直下 ）
   • 最下部近くにある add_executable と target_link_libraries の 既存のテンプレートのコメントを外して編集します.
   • これにより，これらのルールが正しい指示で確実に定義され， 適切な構文を憶えやすくなります．

   注: テンプレートコードの target_link_libraries のように， ほとんどの CMakeLists ルールは 複数の行に分けて記述するできます．

2. 本パッケージのフォルダで src/vision_node.cpp というファイルを作成します． （ gedit を利用 ）

3. ros のヘッダを加えます． （ ros.h のインクルード ）

   /**
   **  Simple ROS Node
   **/
   #include <ros/ros.h>
   

4. main 関数を加えます． （ 典型的な C++ プログラム ）

   /**
   **  Simple ROS Node
   **/
   #include <ros/ros.h>
   
   int main(int argc, char* argv[])
   {
   
   }
   

5. ROS ノードを初期化します． （ main 関数内 ）

   /**
   **  Simple ROS Node
   **/
   #include <ros/ros.h>
   
   int main(int argc, char* argv[])
   {
     // This must be called before anything else ROS-related
     ros::init(argc, argv, "vision_node");
   }
   

6. ROS ノードハンドルを作成します．

   /**
   **  Simple ROS Node
   **/
   #include <ros/ros.h>
   
   int main(int argc, char* argv[])
   {
     // This must be called before anything else ROS-related
     ros::init(argc, argv, "vision_node");
   
     // Create a ROS node handle
     ros::NodeHandle nh;
   }
   

7. "Hello World" メッセージを ROS プリントツールを用いて表示します．

   /**
   **  Simple ROS Node
   **/
   #include <ros/ros.h>
   
   int main(int argc, char* argv[])
   {
     // This must be called before anything else ROS-related
     ros::init(argc, argv, "vision_node");
   
     // Create a ROS node handle
     ros::NodeHandle nh;
   
     ROS_INFO("Hello, World!");
   }
   

8. 自動的にプログラムから抜けないようにして， ノードが走っている状態を維持します．

   /**
   **  Simple ROS Node
   **/
   #include <ros/ros.h>
   
   int main(int argc, char* argv[])
   {
     // This must be called before anything else ROS-related
     ros::init(argc, argv, "vision_node");
   
     // Create a ROS node handle
     ros::NodeHandle nh;
   
     ROS_INFO("Hello, World!");
   
     // Don't exit the program.
     ros::spin();
   }
   

   ROS_INFO は logging methods の1つです．

   • メッセージをターミナルに出力し， 他のノードから見られるように /rosout トピックに送信します．
   • DEBUG, INFO, WARNING, ERROR, FATAL の 5つのレベルのログがあります．
   • 他のログレベルを使用するには ROS_INFO または ROS_INFO_STREAM の INFO を適切なレベルに置き換えます．
   • printf 形式のログには ROS_INFO を使用し， cout 形式のログには ROS_INFO_STREAM を使用します．

9. ターミナルのウィンドウで catkin build を実行して， プログラム（ノード）をビルドします．

   • catkin_ws（または任意のサブディレクトリ）の中から catkin build を実行しなければならないこと を忘れないでください．
   • myworkcell_core にあるプログラム， ライブラリなどをすべてビルドします．
   • 今回は1つの ROS ノード vision_node だけです．

ノードを実行する

1. ターミナルを開いて ROS マスターを起動します．

   roscore
   

   ROS マスターは ROS ノードが働き始める前に 起動しておく必要があります．

2. 2つ目のターミナルで 本演習のノードを実行します．

   • 前回の演習で .bashrc ファイルに追記して devel/setup.bash が新しいターミナルで 自動的に実行されるようにしました．

   • これによりビルドした結果が 新しいターミナルのセッションに自動的に反映されます．

   • 既に起動していたターミナルを利用する場合は， 新しいノードを追加した後に 手動でセットアップファイルを実行する必要があります．

     source ~/catkin_ws/devel/setup.bash
     

3. ノードを実行します．

   rosrun myworkcell_core vision_node
   

   これは今作成したプログラムを実行します． 入力のスピードアップとエラーを減らすために Tab キーを使用することを忘れないでください．

4. 3つ目のターミナルで どのようなノードが走っているのかを確認します．

   rosnode list
   

   /rosout ノードに加えて， 新しい /vision_node が リストされているはずです．

5. rosnode kill /vision_node と入力してください．

   ノードが停止します．

   注: 現在のターミナルウィンドウで 実行中のノードを停止するには Ctrl+C を使用する方が一般的です．

チャレンジ

目標: あなたの名前を出力するようにノードを変更します． そのためにはビルドプロセスを再度実行する必要があります．

トピックとメッセージ

ROS のトピックとメッセージの概念について見てゆきます．

モチベーション

私たちが見る最初のタイプの ROS 通信は 「トピック」と呼ばれるチャンネルを介して送信される 「メッセージ」という一方向通信です． 通常，1つのノードはトピックにメッセージを発行 （ publish／パブリッシュ ）し， 別のノードは同じトピックのメッセージを購読 （ subscribe／サブスクライブ ）します． 本演習では，既存のパブリッシャ（トピック／メッセージ）を購読する 「サブスクライバ・ノード」を作成します．

リファレンス

• Writing a Simple Publisher and Subscriber (C++)
• シンプルな配信者(Publisher)と購読者(Subscriber)を書く(C++)

追加情報とリソース

• Understanding ROS Topics
• Examining the Simple Publisher and Subscriber
• Creating a ROS msg and srv

• ROSトピックの理解
• シンプルなPublisherとSubscriberを実行してみる
• ROSのmsgとsrvを作る

Scan-N-Plan アプリケーション: 演習問題

基本となる ROS ノードが作成できたので， このノードで構築していきます． ノード内にサブスクライバを作成します．

最初の ROS サブスクライバを作成すること を目標としています．

1. まず最初にメッセージ構造について学びます．
2. また，トピック名も決定する必要があります．
3. 最後にサブスクライバとして機能する C++ コードを書きます．

Scan-N-Plan アプリケーション: ガイダンス

依存パッケージへの fake_ar_publisher の追加

1. 先ほどダウンロードした fake_ar_publisher パッケージを探します．

   rospack find fake_ar_publisher
   

2. 演習パッケージの CMakeLists.txt ファイル （ ~/catkin_ws/src/myworkcell_core/CMakeLists.txt ） を編集します．

   既存のルールのコメントアウト を解除したり編集したりして， テンプレートファイルの 一致する箇所で次の変更を行います．

   1. fake_ar_publisher パッケージを見つけるために cmake に指示します．

      ## Find catkin macros and libraries
      ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
      ## is used, also find other catkin packages
      find_package(catkin REQUIRED COMPONENTS
        roscpp
        fake_ar_publisher
      )
      

   2. パブリッシャのための catkin のランタイム依存関係 を追加します．

      ## The catkin_package macro generates cmake config files for your package
      ## Declare things to be passed to dependent projects
      ## LIBRARIES: libraries you create in this project that dependent projects also need
      ## CATKIN_DEPENDS: catkin_packages dependent projects also need
      ## DEPENDS: system dependencies of this project that dependent projects also need
      catkin_package(
       #  INCLUDE_DIRS include
       #  LIBRARIES myworkcell_core
         CATKIN_DEPENDS
           roscpp
           fake_ar_publisher
      #  DEPENDS system_lib
      )
      

   3. add_executable ルールの 下に ある add_dependencies 行のコメント の解除と編集をしてください．

      add_dependencies(vision_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
      

3. 本演習のパッケージの package.xml に依存関係を追加します．

   <depend>fake_ar_publisher</depend>
   

4. 演習パッケージの catkin ワークスペースに cd で移動します．

   cd ~/catkin_ws
   

5. パッケージをビルドしてから， 変更を現在のターミナルに反映させるために セットアップファイルを実行します．

   catkin build
   source ~/catkin_ws/devel/setup.bash
   

6. ターミナルで rosmsg list と入力してください．

   表示されたリストに fake_ar_publisher/ARMarker を見つけることができるはずです．

   あるパッケージにある メッセージだけを表示させたい場合には rosmsg package <package_name> とします．

7. rosmsg show fake_ar_publisher/ARMarker と入力してください．

   ターミナルはメッセージ内の フィールドのタイプと名前を返します．

   • header フィールドの下にある 3つのフィールドはインデントされており， これらが std_msgs/Header メッセージ型のメンバ であることを示しています．

パブリッシャ・ノードの実行

1. ターミナルで rosrun fake_ar_publisher fake_ar_publisher_node と入力してください．

   プログラムの起動とメッセージの配信を開始する 様子が見られるはずです．

2. もう1つのターミナルで rostopic list と入力してください．

   トピックのリストに /ar_pose_marker があるはずです．

   rostopic type /ar_pose_marker を入力するとメッセージのタイプが返されます．

3. rostopic echo /ar_pose_marker と入力してください．

   ターミナルは各メッセージのフィールドを --- 行で区切って表示します．

   Ctrl+C を押して終了します．

4. rqt_plot と入力してください．

   1. ウィンドウが開いたら "Topic:" 欄に /ar_pose_marker/pose/pose/position/x と入力して "+" ボタンをクリックしてください． X の値がプロットされるはずです．
   2. トピック欄に /ar_pose_marker/pose/pose/position/y と入力して 追加ボタンをクリックしてください． X と Y の値が両方グラフになるはずです．
   3. ウィンドウを閉じて下さい．

5. パブリッシャ・ノードは 次のタスクでも用いますので そのまま実行しておいてください．

サブスクライバ・ノードの作成

1. vision_node.cpp ファイルを編集します．

2. メッセージ型のヘッダファイルをインクルードします．

   #include <fake_ar_publisher/ARMarker.h>
   

3. トピックからメッセージを受け取ったときに実行 （コールバック）されるコードを追加します．

   class Localizer
   {
   public:
     Localizer(ros::NodeHandle& nh)
     {
         ar_sub_ = nh.subscribe<fake_ar_publisher::ARMarker>("ar_pose_marker", 1,
         &Localizer::visionCallback, this);
     }
   
     void visionCallback(const fake_ar_publisher::ARMarkerConstPtr& msg)
     {
         last_msg_ = msg;
         ROS_INFO_STREAM(last_msg_->pose.pose);
     }
   
     ros::Subscriber ar_sub_;
     fake_ar_publisher::ARMarkerConstPtr last_msg_;
   };
   

4. コールバックをトピックに接続するコード を main() の中に追加します．

   int main(int argc, char** argv)
   {
     ...
     // The Localizer class provides this node's ROS interfaces
     Localizer localizer(nh);
   
     ROS_INFO("Vision node starting");
     ...
   }
   

   • "Hello World" と出力する機能は 残しても残さなくてもどちらでも構いません．
   • これらの新しい行は NodeHandle の宣言よりも 下でなければなりませんので nh は実際に定義されています．
   • ros::spin() コールは必ず保持してください． 通常 main ルーチンの最後の行にします． ros::spin() の後のコードは ノードがシャットダウンされるまで実行されません．

5. catkin build を実行した後に rosrun myworkcell_core vision_node を実行してください．

6. パブリッシャーから得た座標が表示されます．

7. Ctrl+C でパブリッシャ・ノードを停止します． サブスクライバは情報表示を停止するはずです．

8. パブリッシャ・ノードを再び起動します．

   サブスクライバは 新しくプログラムを実行したときと同じように メッセージの表示を継続するはずです．

   • これはシステム全体に影響を与えずに 個々のノードを再起動できるようにする ROS の重要な機能です．

1. 新しいターミナルで rqt_graph と入力してください．

   次のようなウィンドウが表示されるはずです．

• ウィンドウ内の四角形は システム上で現在利用可能なトピックを示しています．
• 楕円形は ROS ノードです．
• ノードから出ている矢印は ノードがパブリッシュするトピックを示し， ノードに入っている矢印は ノードがサブスクライブするトピックを示します．

セッション 2 - 基本的な ROS の使用法

スライド

サービス

本演習では .srv ファイルを定義してカスタムサービスを作成します． そしてこのサービスを利用するためのサーバーおよびクライアントノードを作成します．

モチベーション

前回，演習を行った最初のタイプの ROS 通信は 「トピック」と呼ばれるチャンネルを介して送信される 「メッセージ」と呼ばれる一方向の相互作用でした．
ここでは異なる通信タイプ， あるノードから別のノードへの要求と そのノードから最初のノードへの応答を介した 双方向の相互通信の使い方を学習します． 本演習ではサービスサーバ（リクエストを待って応答を出します）と クライアント（情報を要求してから応答を待つ）を作成します．

リファレンス

• Writing a Simple Service and Client (C++)
• C++でシンプルなサービスとクライアントを書く

追加情報とリソース

• Creating a ROS msg and srv
• Understanding ROS Services and Parameters
• Examining the Simple Service and Client

• ROSのmsgとsrvを作る
• ROSのサービスとパラメータを理解する
• シンプルなサービスとクライアントを実行してみる

Scan-N-Plan アプリケーション: 演習問題

いくつかの情報を購読するベースとなる ROS ノードが既にあるので このノード上に機能を加えていきます． 更に，このノードを別の「メイン」ノードの サブ機能として使用したいと考えています． 元の視覚ノードは AR 情報を購読して メインのワークセルノードからの 要求に返信するようにします．

目標はより複雑なノードシステムを作成することです．

1. 視覚ノードをアップデートして サービス・サーバを組み込みます．
2. ゆくゆくは Scan-N-Plan アプリケーション を実行する新しいノードを作成します．
   • まず，新しいノード（ myworkcell_core ）を サービスクライアントとして作成します． 後にその機能を拡張してゆきます．

Scan-N-Plan アプリケーション: ガイダンス

サービス定義の作成

1. fake_ar_publisher パッケージにある メッセージファイルと同じように サービス定義を作成しますので， サービスファイルを作成する必要があります．

   サービスファイルの一般的な構造は 次のようになっています．

   #request
   ---
   #response
   

2. myworkcell_core パッケージの中に srv という名前のフォルダを作成します． （ パッケージの src フォルダと同じ階層 ）

   cd ~/catkin_ws/src/myworkcell_core
   mkdir srv
   

3. srv フォルダ内に LocalizePart.srv というファイルを （ gedit や QT で ）作成します．

4. ファイルの中に base_frame という名前の string 型の要求と pose という名前の geometry_msgs/Pose 型の応答を 上に概説したようにサービスを定義してください．

   #request
   string base_frame
   ---
   #response
   geometry_msgs/Pose pose
   

5. パッケージの CMakeLists.txt と package.xml を編集して， キーパッケージに依存関係を追加します．

   • message_generation は前のステップで作成した .srv ファイルから C++ コードをビルドする必要があります．
   • message_runtime は 新しいメッセージのランタイム依存関係を提供します．
   • geometry_msgs は サービス定義で使用される Pose メッセージタイプを提供します．

   1. パッケージの CMakeLists.txt ファイルを編集して， 新しい ビルドタイム 依存関係を 既存の find_package ルールに追加してください．

      find_package(catkin REQUIRED COMPONENTS
        roscpp
        fake_ar_publisher
        geometry_msgs
        message_generation
      )
      

   2. 同じく CMakeLists.txt に 新しい ランタイム 依存関係を 既存の catkin_package ルールに追加します．

      catkin_package(
      #  INCLUDE_DIRS include
      #  LIBRARIES myworkcell_node
        CATKIN_DEPENDS
          roscpp
          fake_ar_publisher
          message_runtime
          geometry_msgs
      #  DEPENDS system_lib
      )
      

   3. package.xml ファイルを編集して 適切なビルド・実行依存関係を追加します．

      <build_depend>message_generation</build_depend>
      <exec_depend>message_runtime</exec_depend>
      <depend>geometry_msgs</depend>
      

6. パッケージの CMakeLists.txt を編集して 新しいサービスファイルを生成するルールを追加します．

   1. 前に定義した LocalizePart サービスを参照するために， 次の CMakeLists.txt ルールのコメントの解除と 編集を行ってください．

      ## Generate services in the 'srv' folder
      add_service_files(
         FILES
         LocalizePart.srv
      )
      

   2. メッセージとサービスの生成を有効にするには， 次の CMakeLists.txt ルールのコメントの解除と 編集を行ってください．

      ## Generate added messages and services with any dependencies listed here
      generate_messages(
         DEPENDENCIES
         geometry_msgs
      )
      

7. これでサービスをパッケージに定義することができましたので， サービスを生成するコードを Build できます．

   catkin build
   

   注: Qt を用いても良いです．

サービスサーバ

1. vision_node.cpp を編集します． ROS Wiki （ 日本語 ） が基になっています．

2. 今しがた作成したサービスのヘッダーを追加します．

    #include <myworkcell_core/LocalizePart.h>
   

3. 他の Localizer クラスメンバ変数の近くに メンバ変数（ 型:ServiceServer，名前:server_ ） を追加してください．

   ros::ServiceServer server_;
   

4. Localizer クラスのコンストラクタで ROS マスターにこのサービスを告知してください．

   server_ = nh.advertiseService("localize_part", &Localizer::localizePart, this);
   

5. 上記の advertiseService コマンドは localizePart というサービスコールバックを参照していました． Localizer クラスにこの名前の空のブール関数を作成します．

   リクエストとレスポンスのタイプは LocalizePart.srv ファイルで定義されている ことを思い出してください． ブール関数の引数はリクエストと応答の型で， 一般的な構造は Package::ServiceName::Request または Package::ServiceName::Response です．

   bool localizePart(myworkcell_core::LocalizePart::Request& req,
                     myworkcell_core::LocalizePart::Response& res)
   {
   
   }
   

6. 次に localizePart コールバック関数にコードを追加して サービスレスポンスを記入します．

   最終的にこのコールバックは fake_ar_publisher（ visionCallback 内 ） から受け取ったポーズをサービスリクエストで 指定されたフレームに変換します．

   今回はフレーム変換を飛ばして fake_ar_publisher から受け取ったデータをただ渡します． fake_ar_publisher（ last_msg_ に保存 ） から受け取ったポーズ測定値を 直接サービス応答にコピーします．

   bool localizePart(myworkcell_core::LocalizePart::Request& req,
                     myworkcell_core::LocalizePart::Response& res)
   {
     // Read last message
     fake_ar_publisher::ARMarkerConstPtr p = last_msg_;  
     if (!p) return false;
   
     res.pose = p->pose.pose;
     return true;
   }
   

7. 無駄な情報が画面がいっぱいにならないように visionCallback 関数内の ROS_INFO_STREAM の呼び出し をコメントアウトする必要があります．

8. 更新された vision_node をビルドして コンパイルエラーがないことを確認します．

サービスクライアント

1. myworkcell_node.cpp という名前の新しいノードを 同じ myworkcell_core パッケージ内に）作成します．

   これは最終的にメインの「アプリケーションノード」になって， Scan-N-Plan タスクの動作順序を制御します． 最初に実装する動作は， 上で作成した視覚ノードの LocalizePart サービスから AR ターゲットの位置を要求することです．

2. LocalizePart サービスのヘッダーと同様に， 標準の ros ヘッダファイルもインクルードしてください．

   #include <ros/ros.h>
   #include <myworkcell_core/LocalizePart.h>
   

3. 一般的な ROS ノード初期化方法を用いて 標準的な C++ メイン関数を作成します．

   int main(int argc, char **argv)
   {
     ros::init(argc, argv, "myworkcell_node");
     ros::NodeHandle nh;
   
     ROS_INFO("ScanNPlan node has been initialized");
   
     ros::spin();
   }
   

4. C++ クラス "ScanNPlan" に myworkcell_node のほとんどの機能を記述します．

   空のコンストラクタと 内部/プライベート変数による プライベート空間を有する このクラスのスケルトン構造を作成します．

   class ScanNPlan
   {
   public:
     ScanNPlan(ros::NodeHandle& nh)
     {
   
     }
   
   private:
     // Planning components
   
   };
   

5. 新しい ScanNPlan クラス内で クラスのプライベートメンバー変数として ROS ServiceClient を定義します．

   前に定義したのと同じサービス名（"localize_part"）を使用して ScanNPlan コンストラクタ内で ServiceClient を初期化します．

   ScanNPlan クラス内に引数なしで start という名前の void 関数を作成します． これにはほとんどの本アプリケーションの ワークフローを含むようになります．

   現時点ではこの関数は LocalizePartサービスを呼び出し， 応答を出力します．

   class ScanNPlan
   {
   public:
     ScanNPlan(ros::NodeHandle& nh)
     {
       vision_client_ = nh.serviceClient<myworkcell_core::LocalizePart>("localize_part");
     }
   
     void start()
     {
       ROS_INFO("Attempting to localize part");
       // Localize the part
       myworkcell_core::LocalizePart srv;
       if (!vision_client_.call(srv))
       {
         ROS_ERROR("Could not localize part");
         return;
       }
       ROS_INFO_STREAM("part localized: " << srv.response);
     }
   
   private:
     // Planning components
     ros::ServiceClient vision_client_;
   };
   

6. myworkcell_node の main 関数に戻り， ScanNPlan クラスのオブジェクトをインスタンス化し， オブジェクトのstart 関数を呼び出します．

   ScanNPlan app(nh);
   
   ros::Duration(.5).sleep();  // wait for the class to initialize
   app.start();
   

7. パッケージの CMakeLists.txt を編集して 関連する依存関係を持つ 新しい（実行可能な）ノードを構築します．

   適切なセクションに vision_node のマッチングルールの直下に 次のルールを追加してください．

   add_executable(myworkcell_node src/myworkcell_node.cpp)
   
   add_dependencies(myworkcell_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
   
   target_link_libraries(myworkcell_node ${catkin_LIBRARIES})
   

8. ノードをビルドして コンパイル時エラーが出ないことを確認してください．

   catkin build
   

   注: Qt を用いても良いです．

サービスの利用

1. 次の各コマンドを 各々のターミナルに入力してください．

   roscore
   rosrun fake_ar_publisher fake_ar_publisher_node
   rosrun myworkcell_core vision_node
   rosrun myworkcell_core myworkcell_node
   

アクション

本演習は標準的な ROS-I トレーニングクラスワークフローの一部ではありません． 標準の ROS チュートリアル（下記リンク）に従ってください．

---

ROSチュートリアル: C++ アクションクライアント/サーバーの使用法

• Writing a Simple Action Server using the Execute Callback
• Writing a Simple Action Client

• コールバックを用いてシンプルなアクションサーバーを書く
• シンプルなアクションクライアントを書く

Launch ファイル

本演習では起動（ launch ）ファイルを使用してノードのグループを一度に起動する方法を見てゆきます．

モチベーション

ROS アーキテクチャは システム内の組織の基本単位として 「ノード」を使用することを奨励しています． そのため多くのノードを動作させる必要が出てきますので アプリケーションが急増します． いちいち新しいターミナルを開いて 各ノードを個別に実行することは事実上困難になります．
こうなるとノードのグループを一度に起動するツールがあると助かります． ROS の "launch" ファイルは そのようなツールの1つです． "launch" ファイルは roscore の起動・停止さえ扱ってくれます．

リファレンス

• Example .launch XML Config Files

追加情報とリソース

• Roslaunch XML Specification
• Debugging and Launch Files

• ja/roslaunch/XML
• Valgrind か GDBでnodeをroslaunchする方法

Scan-N-Plan アプリケーション: 演習問題

本演習では次のことを行います．

1. パッケージ myworkcell_support を新しく作成する．
2. launch ディレクトリを このパッケージ内に新しく作成する．
3. このディレクトリ内に 下記機能を持つファイル workcell.launch を新しく作成する．
   1. fake_ar_publisher を起動する．
   2. vision_node を起動する．

また myworkcell_core ノードを 他のものと一緒に起動するか 別々に起動するか も選択できるようにします．

一般的には 2つの主要な launch ファイル でシステムを構成します． この例では fake_ar_publisher と vision_node は「環境」ノードですが， myworkcell_nodeは「アプリケーション」ノードです．

1. 「環境」launch ファイル: ドライバ／動作計画ノード，設定データなど
2. 「アプリケーション」launch ファイル: 特定のアプリケーションに対してシーケンス動作を実行

Scan-N-Plan アプリケーション: ガイダンス

1. ワークスペース内に myworkcell_core に依存関係をもつ 新しいパッケージ myworkcell_support を作成してください．

   ROS が新しいパッケージを見つけることができるように ワークスペースをビルドした後に設定を反映させます．

   cd ~/catkin_ws/src
   catkin create pkg myworkcell_support --catkin-deps myworkcell_core
   catkin build
   source ~/catkin_ws/devel/setup.bash
   

2. launch ファイル用のディレクトリを作成します． （新しい myworkcell_support パッケージ内）

   roscd myworkcell_support
   mkdir launch
   

3. 新しいファイル workcell.launch （ launchディレクトリ内 ）を 次の XML スケルトンで作成します．

   <launch>
   
   </launch>
   

4. 演習問題に記載されている ノードを起動するための行を追加します．

   詳細はリファレンスを見てください．

   <node name="fake_ar_publisher" pkg="fake_ar_publisher" type="fake_ar_publisher_node" />
   <node name="vision_node" pkg="myworkcell_core" type="vision_node" />
   

   • 留意: すべての起動ファイルコンテンツは <launch> ... </launch> タグのペアの間にある 必要があります．

5. launch ファイルをテストします．

   roslaunch myworkcell_support workcell.launch
   

   注: roscore と2つのノードは自動的に起動しました． Ctrl+C を押して launch ファイルによって開始された全てのノードを閉じます． 実行中のノードがない場合 roscore も停止します．

6. 通常のメッセージは コンソールウィンドウに表示されませんでした． launch ファイルはデフォルトでは 重大度 ERROR 未満のコンソール出力が抑制されます． 通常のテキスト出力を復元するには launch ファイルの各ノードに output="screen" 属性を追加します．

   <node name="fake_ar_publisher" pkg="fake_ar_publisher" type="fake_ar_publisher_node" output="screen"/>
   <node name="vision_node" pkg="myworkcell_core" type="vision_node" output="screen" />
   

パラメータ

ノードを設定するためのROSパラメータを見ていきます．

モチベーション

本チュートリアル（またはあなたのキャリア）のこの時点で， 幾度となく int main(int argc, char** argv) という文をタイプしたことでしょう． main への引数は スコープ外のシステムがそのプログラムを理解して 特定のタスクを行うようにプログラムを構成できる手段です． これらは コマンドライン・パラメータ です．

ROS エコシステムには ノードのグループ全体を構成するための 類似のシステムがあります． それは roscore の一部として起動される Key-Value ストレージプログラムです． 設定パラメータをノードに個別に渡すの （ノードの登録先を特定するなど）にも最適ですが， はるかに複雑な項目にも利用できます．

リファレンス

• Understanding Parameters
• rosparam を使う

追加情報とリソース

• Roscpp tutorial
• Private Parameters
• Parameter Server

• ノードハンドルからプライベートネームで接続する
• ja/Parameter Server

Scan-N-Plan アプリケーション: 演習問題

前回の演習で 次の定義を持つサービスを追加しました．

# request
string base_frame
---
# response
geometry_msgs/Pose pose

これまではリクエストフィールドにある base_frame を全く使用していませんでした． この演習では ROS パラメータを使用して このフィールドを設定します． 次のことを行ってください．

1. 通常のものに加えて， プライベートノード・ハンドルを myworkcell_node の main メソッドに追加します．
2. プライベートノード・ハンドルを使用して， パラメータ base_frame を読み込み， それをローカルの文字列オブジェクトに格納します．
   • パラメータが指定されていない場合は デフォルトでパラメータ "world" に設定されるようにします．
3. vision_node へのサービスコールを行うときは， このパラメータを使用して request::base_frame フィールドに記入してください．
4. 新しい値を初期化するために launch ファイルに <param> タグを追加します．

Scan-N-Plan アプリケーション: ガイダンス

1. myworkcell_node.cpp を開いて編集します．

2. 新しい ros::NodeHandle オブジェクト を main 関数に追加し， そのパラメータをプライベートにします．

   詳細については ROS Wiki の関連する箇所を参照してください．

   ros::NodeHandle private_node_handle ("~");
   

3. テンポラリの文字列オブジェクト std::string base_frame; を作成し， プライベートノードハンドルの [API]（http://docs.ros.org/indigo/api/roscpp/html/classros_1_1NodeHandle.html） を使用して，パラメータ "base_frame" を読み込みます．

   private_node_handle.param<std::string>("base_frame", base_frame, "world"); // parameter name, string object reference, default value
   

   • base_frame パラメータは private_node_handle が宣言された後， app.start() が呼び出される前 に読み込まなければなりません．

4. base_frame 引数を受け入れる myworkcell_node の start 関数にパラメータを追加し， パラメータからの値を サービスリクエストに割り当てます．

   main() ルーチンの app.start 呼び出しを更新して， パラメータサーバから読み込んだ base_frame の値を渡すようにしてください．

   void start(const std::string& base_frame)
   {
     ...
     srv.request.base_frame = base_frame;
     ROS_INFO_STREAM("Requesting pose in base frame: " << base_frame);
     ...
   }
   
   int main(...)
   {
     ...
     app.start(base_frame);
     ...
   }
   

   • srv.request はサービス呼び出しに渡す 前に 設定する必要があります．（ vision_client.call(srv) ）

5. これで，既存の workcell.launch ファイルに myworkcell_node を追加することで， base_frame パラメータを launch ファイルから設定できるようになります． 動作計画（モーション・プランニング）のためには， vision_node がワールド・フレームに対するターゲットの位置 を返すことが望ましいです． デフォルト値としてそうはなっているのですが， 一応 launch ファイルで指定しておきます．

   <node name="myworkcell_node" pkg="myworkcell_core" type="myworkcell_node" output="screen">
     <param name="base_frame" value="world"/>
   </node>
   

6. システムを実行して試してみてください．

   catkin build
   roslaunch myworkcell_support workcell.launch
   

   • 実行中のノードを停止するには Ctrl+C を押してください．
   • launch ファイルを編集して， base_frame パラメータを変更してみてください． （ 例: "test2" ）
   • workcell.launch を再起動して， "request frame" が変わったことを見てみてください．
     • リクエスト・フレームを扱うために vision_node を（まだ）更新していないため， レスポンス・フレームは変更されません． vision_node は（今のところ）常に同じフレームを返します．
   • base_frame を "world" に戻してください．

セッション 3 - マニュピレータの制御

スライド

URDF 入門

ロボットを URDF 形式で記述する方法を見ていきます．

モチベーション

ROS とその協同ソフトウェアの 最もクールで便利な機能の多くには 干渉チェックや動作軌道計画などが含まれています． コードに依存しなく人間が判読可能な方法で ロボットと作業セルのジオメトリを記述することは便利なことが多いです． これはテキストによる CAD の記述のように考えてください．

「部品1は部品2から1メートル左にあり，表示用ポリゴンメッシュがあります」

Unified Robot Description Format（URDF）は， 今日，これらのフォーマットの中で最も一般的です． 本演習では簡単なロボットセルを作成していきます． このロボットセルは後で実際に使用するために発展させていきます．

リファレンス

• Building a Visual Robot Model with URDF from Scratch

追加情報とリソース

• XML Specification
• ROS Tutorials
• XACRO Extensions
• SolidWorks to URDF Exporter

Scan-N-Plan アプリケーション: 演習問題

Scan-N-Plan アプリケーションの ソフトウェアスケルトンが既にあるので， 次のステップを踏んで， そこにいくつかの物理的なコンテキスト を追加していきましょう．

本演習で説明するジオメトリは次の目的に利用されます．

1. 干渉チェックの実施
2. ロボットの運動学的構成の把握
3. 座標変換の計算

目標は次の特徴を持つワークセルを記述することです．

1. world という名前の 全ての基となるフレーム
2. table ジオメトリ（平坦な直方体）を有する 独立したフレーム
3. camera_frame という名前の Z 軸が world の Z 軸に対して反対方向に 付けられたフレーム（ジオメトリは任意）

Scan-N-Plan アプリケーション: ガイダンス

1. コード以外の設定ファイルは それぞれのサポート（"support"）パッケージ内 に置くのが通例です． URDF は通常，サポートパッケージの サブフォルダ "urdf/" に入れるようにしています．

   abb_irb2400_support パッケージを参考にしてください．

   アプリケーションサポートパッケージに urdf サブフォルダを追加してください。

2. myworkcell_support/urdf/ フォルダ内に 新しい workcell.urdf ファイルを作成し， 次の XML スケルトンを挿入します．

   <?xml version="1.0" ?>
   <robot name="myworkcell" xmlns:xacro="http://ros.org/wiki/xacro">
   </robot>
   

3. 必要なリンクを追加します．

   ABB2400 の irb2400_macro.xacro の例を参考にしてください．

   すべての URDF タグは <robot> ... </robot> の タグの間に 置かなければならないことを留意してください．

   1. world フレームを「仮想リンク」として追加してください． （ジオメトリなし）

      <link name="world"/>
      

   2. tableフレームを追加して， <collision> と <visual> の両方の <geometry> タグを指定してください．

      XML で記述された box 型です．

      <link name="table">
        <visual>
          <geometry>
            <box size="1.0 1.0 0.05"/>
          </geometry>
        </visual>
        <collision>
          <geometry>
            <box size="1.0 1.0 0.05"/>
          </geometry>
        </collision>
      </link>
      

   3. 仮想リンクとして camera_frame を追加してください．（ジオメトリなし）

      <link name="camera_frame"/>
      

   4. リンクを固定関節のペアで接続します． world_to_camera 関節を rpy タグを使用して 本演習問題の説明に従って向きを設定します．

      <joint name="world_to_table" type="fixed">
        <parent link="world"/>
        <child link="table"/>
        <origin xyz="0 0 0.5" rpy="0 0 0"/>
      </joint>
      
      <joint name="world_to_camera" type="fixed">
        <parent link="world"/>
        <child link="camera_frame"/>
        <origin xyz="-0.25 -0.5 1.25" rpy="0 3.14159 0"/>
      </joint>
      

   5. 追加したリンクが意図したとおりになっているか URDF を視覚化して確認することができます.

      roslaunch urdf_tutorial display.launch model:=<RELATIVE_PATH_TO_URDF>
      

      RViz に何も表示されない場合は RViz のベースフレーム（上部の左パネル）を モデル内にあるリンクの名前に変更する必要があります．

作業セル XACRO

単純なロボット作業セルを表す XACRO ファイルを作成します． これにより URDF と XACRO の両方の要素を見ることができます．

モチベーション

要素がほんの数個よりも多い URDF を書いていると， すぐにそれが苦痛になってくるかと思います． ワークスペースのリンクやジョイントの束をコピーして貼り付け， その名前をわずかに変更して複製したアイテムやファイルは膨大になってしまいます． 起動時に把握される（またはされない）間違いが簡単に発生してしまいます．

コンポーネントを一度定義すれば複製無しで どこででも再利用可能といったような， プログラミング言語で行っているような指示方法があれば助かります． プログラムでは関数とクラスがそれを行い， XACRO マクロは URDF のためにそれを行います． XACRO には他にも （#include 的な）ファイル・インクルード・システム， 定数変数， 数式表現評価（ 例えば 1.57 ではなく PI/2.0 ） などの優れた機能もあります．

リファレンス

• Using Xacro to Clean Up a URDF File

追加情報とリソース

• Xacro Extension Documentation
• Creating a URDF for an Industrial Robot

Scan-N-Plan アプリケーション: 演習問題

前回の演習では静的ジオメトリのみで構成される 作業セル（workcell）を作成しました． 今回は XACRO ツールを使用して UR5 ロボット assembly を追加します．

具体的には次のように行っていきます．

1. 前回作成した *.urdf ファイルを xacro 拡張子を持つ XACRO ファイルに変換します．
2. UR5 の XACRO マクロ定義を含むファイルを インクルードします．
3. 作業スペースで UR5 をインスタンス化し， それを table リンクに接続します．

Scan-N-Plan アプリケーション: ガイダンス

1. 前回の演習の workcell.urdf のファイル名を workcell.xacro に変更してください．

2. ur_description パッケージを ROS 環境に導入してください．

   いくつかの方法があります．

   1. debian パッケージをインストールする．

      sudo apt install ros-kinetic-ur-description ros-kinetic-ur-kinematics
      

   2. GitHub から catkin ワークスペースにクローンする．

      cd ~/catkin_ws/src
      git clone https://github.com/ros-industrial/universal_robot.git
      catkin build
      source ~/catkin_ws/devel/setup.bash
      

   description パッケージが各 "module"，"part" または "assembly" を それ自身のファイルに入れることはよくあります． 多くの場合，パッケージは特定の部品を持つ 全体のセルを定義する以外のファイルも定義していて， それらを簡単に視覚的に調べることができます． UR パッケージは UR5（ ur5_robot.urdf.xacro ）のファイルを定義します． これは本演習にとって非常に良い例となっています．

3. UR5 マクロを実装している xacro ファイルを探し， 新しく名前を変更した workcell.xacro ファイルに組み込みます．

   workcell.xacro ファイル最上部の <robot> タグの下に 次のインクルードする行を追加してください．

   <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />
   

   UR5 定義ファイルや Xacro マクロを定義している他のファイルを読んでいると， 要素名に ${prefix} をたくさん使っていることが見て取れると思います． Xacro は実行時に "${}" 内のものを評価します． 基本的な数式を扱うことができ， プロパティ（ala-global変数）やマクロパラメータを使って変数を参照することができます． ほとんどのマクロは"prefix" パラメータを取得して， ユーザが前述のマクロの複数のインスタンスを作成できるようにしています． これは最終的な URDF 要素名を唯一なものにするためのメカニズムですが， そうならなかった場合は重複するリンク名を取得し URDF が障害を報告をします．

4. ur5.urdf.xacro ファイルをインクルードしても URDF モデルに実際に UR5 ロボットが作成されるわけではありません． マクロは定義されましたが， マクロを呼び出してロボットのリンクとジョイントを作成する必要があります．

   前述のように prefix タグの使用に注意してください．

   <xacro:ur5_robot prefix="" joint_limited="true"/>
   

   Xacro のマクロはコピー＆ペーストに関する素晴らしいラッパーです． マクロを作ってもそれはリンクと関節の塊です． 世界の他の部分をそのマクロの出力に関連付けなければなりません． マクロを見て，ベースリンクが何で，エンドリンクが何かを確認する必要があります． 願わくば，マクロは ROS Industrial のそれのような標準に準じた ベースリンクは "base_link" と最後のリンクは "tool0" と名づけられると良いでしょう．

5. UR5の base_link を 既存の静的ジオメトリに固定リンクに接続します．

   <joint name="table_to_robot" type="fixed">
     <parent link="table"/>
     <child link="base_link"/>
     <origin xyz="0 0 0" rpy="0 0 0"/>
   </joint>
   

6. 新しい urdf.launch ファイル （ myworkcell_support パッケージ内 ）を作成して， URDF モデルをロードし， オプションで RViz に表示します．

   <launch>
     <arg name="gui" default="true"/>
     <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find myworkcell_support)/urdf/workcell.xacro'" />
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
     <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
       <param name="use_gui" value="$(arg gui)"/>
     </node>
     <node name="rviz" pkg="rviz" type="rviz" if="$(arg gui)"/>
   </launch>
   

7. 作成した launch ファイルを実行して， 更新された URDF を RViz 上で確認します．

   roslaunch myworkcell_support urdf.launch

   • 'Fixed frame' を 'world' に設定し， 左のツリービューに RobotModel と TF の表示を追加して ロボットといくつかの変換座標を表示します．
   • ジョイントスライダを動かして UR5 ロボットの動きを確認してください．

TF による座標変換

座標変換ライブラリである TF で使用されるターミナルコマンドと C++ コマンドについて見ていきます．

モチベーション

実際に自身が動いたり何か他の動きを見たりしなくても便利な 「ロボット」というのはイメージし難いです． 有用な ROS におけるアプリケーションというのは必然的に 部品やロボットリンクまたはツールの位置を 観察する必要のあるコンポーネントを有します． ROS ではこれを行いやすくする 「エコシステム」とライブラリを TF と呼んでいます．

TF は接続されたフレーム間の変換を 時間を遡っても調べることを可能にする基本的なツールです． 例えば「10秒前における A と B の間の変換は何ですか？」 という質問をすることも可能にする便利な道具です．

リファレンス

• Writing a tf listener (C++)

追加情報とリソース

• tf - ROS Wiki
• TF Tutorials
• TF Listener API

• ja/ tf/ - ROS Wiki
• ja/ tf/ Tutorials

Scan-N-Plan アプリケーション: 演習問題

（シミュレートされた）カメラにより取得された部品の姿勢情報は カメラ自体の光学基準フレームとして得られます． ロボットがこのデータを基に何かを行うためには データをロボットの参照フレームに変換する必要があります．

具体的には vision_node 内のサービスコールバックを編集して， 部品姿勢の最新情報を camera_frame から サービスコールの base_frame リクエストフィールドに変換します．

Scan-N-Plan アプリケーション: ガイダンス

1. コアパッケージの依存関係に tf を指定してください．

   • 前の演習のように package.xml（1行）と CMakeLists.txt（2行）を編集します．

2. vision node にクラスメンバ変数として tf::TransformListener オブジェクトを追加します．

   #include <tf/transform_listener.h>
   ...
   tf::TransformListener listener_;
   

3. 取得された目標物の姿勢を 参照したフレーム（"camera_frame"）から サービス・リクエスト・フレームに変換するために， 既存の localizePart メソッドにコードを追加します．

   1. 良かれ悪しかれ ROS はさまざまな数学ライブラリを使用します． geometry_msgs::Pose の over-the-wire 形式を tf::Transform オブジェクトに変換する必要があります．

      tf::Transform cam_to_target;
      tf::poseMsgToTF(p->pose.pose, cam_to_target);
      

   2. リスナ・オブジェクトを用いて request.base_frame と ARMarker メッセージ（これは "camera_frame" 上にあるはずです） からの参照フレームの間の最新の変換を調べます．

      tf::StampedTransform req_to_cam;
      listener_.lookupTransform(req.base_frame, p->header.frame_id, ros::Time(0), req_to_cam);
      

   3. 上で得られた情報を用いて 対象物の姿勢を目標フレームに変換します．

      tf::Transform req_to_target;
      req_to_target = req_to_cam * cam_to_target;
      

   4. サービス応答で変換された姿勢を返します．

      tf::poseTFToMsg(req_to_target, res.pose);
      

4. ノードを実行して変換をテストします．

   catkin build
   roslaunch myworkcell_support urdf.launch
   roslaunch myworkcell_support workcell.launch
   

5. workcell.launch の base_frame パラメータを 例えば "table" などに変更して， workcell.launch ファイルを再起動し， 異なるポーズの結果を見てみてください．

   完了したら "base_frame" パラメータを "world" に戻してください．

MoveIt! パッケージのビルド

産業用ロボットの MoveIt! パッケージを作成します． このパッケージは MoveIt! のモーションコントロールノードを用いて ロボットを使用するために必要な設定ファイルと launch ファイルを作成します． 一般的には C++ コードを それらの MoveIt! パッケージに含むことはしません．

モチベーション

MoveIt! は ROS の自由空間動作計画フレームワークです． それは何かに干渉することなく空間内の2つのポイント間の 動作を計画するための非常に便利で使いやすいツールです． MoveIt! のボンネットの中身は非常に複雑ですが， 多くの ROS ライブラリとは異なり， 本当に素晴らしい魅力的な GUI 操作画面を持っています．

リファレンス

• Create a MoveIt Package for an Industrial Robot

追加情報とリソース

• MoveIt’s Standard Wizard Guide

Scan-N-Plan アプリケーション: 演習問題

前段階で作成した UR5 作業セルの MoveIt! パッケージを生成します． このプロセスのほとんどは MoveIt! Setup Assistant の実行に関連するものです． 演習の最後には次のものができているはずです．

1. 新しいパッケージ myworkcell_moveit_config
2. UR5 の base_link と tool0 間の運動学的な連結で構成された 1つのグループ（"manipulator"）を持つ MoveIt! コンフィグレーション

Scan-N-Plan アプリケーション: ガイダンス

1. MoveIt! Setup Assistant を起動します．

（ tab の補完機能を使ってください．）

```
roslaunch moveit_setup_assistant setup_assistant.launch
```

1. "Create New MoveIt Configuration Package" を選択してから 以前の演習で作成した workcell.xacro を選択して "Load File" してください．

2. 左上のタブを上から下に向かって作業していきます．

   1. 自己干渉マトリクス（ self-collision matrix ）を作成する．

   2. 次の例のように固定された仮想ベース関節を追加します．

      name = 'FixedBase' (arbitrary)
      child = 'world' (should match the URDF root link)
      parent = 'world' (reference frame used for motion planning)
      type = 'fixed'
      

   3. base_link と tool0 間に 運動学的な連結（ kinematic chain ）を指定する manipulator というプランニング・グループを追加します．

      注: ROS命名ガイドライン・要件 に従ってください． どこにも空白を使用しないでください．

      a. キネマティック・ソルバを KDLKinematicsPlugin に設定します．

   4. 動作計画テストのために，いくつかの名前付き位置 （ 例: "home"，"allZeros" など ）を作成します．

   5. 本演習ではエンドエフェクタ，グリッパ，またはパッシブジョイントの追加は考慮不要です．

   6. 著者・管理者情報を入力してください．

      入力は必須ですが有効な内容である必要はありません．

   7. 新しいパッケージを生成し myworkcell_moveit_config という名前を付けます．

      • catkin_ws/src ディレクトリ内にパッケージを作成してください．

   8. 現在の MoveIt! Settup Assistant には いくつかの小さなエラーや異常な動作を引き起こす バグ があります． これらのエラーを修正するには次のことを行ってください．

      1. myworkcell_core_moveit_config/config/ompl_planning.yaml ファイルを編集します．
      2. 各プランナ名にテキスト文字列 kConfigDefault を付加してください．
         • 例: SBL: -> SBLkConfigDefault など

これらのステップの結果， 多数の launch ファイルと設定ファイルを含む 新しいパッケージになります． この時点でも動作計画（モーション・プランニング）は可能ですが， ロボットで動作計画を実行することはできません． 新しい設定を試すには次のようにします．

catkin build
source ~/catkin_ws/devel/setup.bash
roslaunch myworkcell_moveit_config demo.launch

ロボットを動かすための RViz の使用方法については 次回の演習で説明しますので 心配しないでください．

実機での MoveIt! の使用

MoveIt! Setup Assistant は 起動時の一揃いのファイルを生成します．

• ワークスペースの概要をパラメーター・サーバーにロードする．
• 運動学，動作計画一連の ROS サービスと アクションを提供するノード move_group を開始する．
• 視覚化ツール（ RViz など ）のループに 直近に計画された軌道をパブリッシュする内部シミュレータ．

基本的には MoveIt! は 軌道（時間の経過による関節の位置）を定義する ROS メッセージをパブリッシュできますが， その軌道をハードウェアに渡す方法はわかりません．

これを行うにはいくつかの追加ファイルを定義する必要があります．

1. 次の内容の controllers.yaml ファイルを作成する． (myworkcell_moveit_config/config/controllers.yaml)

   controller_list:
     - name: ""
       action_ns: joint_trajectory_action
       type: FollowJointTrajectory
       joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
   

2. joint_names.yaml ファイルを作成する． (myworkcell_moveit_config/config/joint_names.yaml)

   controller_joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
   

3. 既存の controller_manager launch ファイルを完成させる． (myworkcell_moveit_config/launch/myworkcell_moveit_controller_manager.launch.xml)

   <launch>
     <arg name="moveit_controller_manager"
          default="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
     <param name="moveit_controller_manager"
            value="$(arg moveit_controller_manager)"/>
   
     <rosparam file="$(find myworkcell_moveit_config)/config/controllers.yaml"/>
   </launch>
   

4. myworkcell_planning_execution.launch ファイルを myworkcell_moveit_config/launch 内に作成する．

   <launch>
     <!-- The planning and execution components of MoveIt! configured to run -->
     <!-- using the ROS-Industrial interface. -->
   
     <!-- Non-standard joint names:
          - Create a file [robot_moveit_config]/config/joint_names.yaml
              controller_joint_names: [joint_1, joint_2, ... joint_N]
          - Update with joint names for your robot (in order expected by rbt controller)
          - and uncomment the following line: -->
     <rosparam command="load" file="$(find myworkcell_moveit_config)/config/joint_names.yaml"/>
   
     <!-- the "sim" argument controls whether we connect to a Simulated or Real robot -->
     <!--  - if sim=false, a robot_ip argument is required -->
     <arg name="sim" default="true" />
     <arg name="robot_ip" unless="$(arg sim)" />
   
     <!-- load the robot_description parameter before launching ROS-I nodes -->
     <include file="$(find myworkcell_moveit_config)/launch/planning_context.launch" >
      <arg name="load_robot_description" value="true" />
     </include>
   
     <!-- run the robot simulator and action interface nodes -->
     <group if="$(arg sim)">
       <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
     </group>
   
     <!-- run the "real robot" interface nodes -->
     <!--   - this typically includes: robot_state, motion_interface, and joint_trajectory_action nodes -->
     <!--   - replace these calls with appropriate robot-specific calls or launch files -->
     <group unless="$(arg sim)">
       <include file="$(find ur_bringup)/launch/ur5_bringup.launch" />
     </group>
   
     <!-- publish the robot state (tf transforms) -->
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
   
     <include file="$(find myworkcell_moveit_config)/launch/move_group.launch">
       <arg name="publish_monitored_planning_scene" value="true" />
     </include>
   
     <include file="$(find myworkcell_moveit_config)/launch/moveit_rviz.launch">
       <arg name="config" value="true"/>
     </include>
   
   </launch>
   

5. 新しく作成した launch ファイルをテストします．

   roslaunch myworkcell_moveit_config myworkcell_planning_execution.launch
   

RViz を利用した動作計画

本演習では最終的にシミュレーションロボットの 動作計画を行って実行するための MoveIt! の RViz プラグインの使用方法を学習します． MoveIt! と RViz のプラグイン両方に関連する さまざまなオプションや拘束条件を見ていきます．

動作計画環境の起動

1. catkin ワークスペースの環境セットアップを実行してください．

2. 動作計画環境を起動して，ROS-I シミュレータ・ノードに接続します．

   roslaunch myworkcell_moveit_config myworkcell_planning_execution.launch
   

プラグインの表示オプション

1. ディスプレイパネル Motion Planning 内の 次の表示オプションを探して試してみてください．

   • Scene Robot -> Show Robot Visual
   • Scene Robot -> Show Robot Collision
   • Planning Request -> Query Start State
   • Planning Request -> Query Goal State

2. ここでは Show Robot Visual と Query Goal State の表示を有効にして， Show Robot Collision と Query Start State は無効にしてください．

   

3. Panel -> Motion Planning - Trajectory Slider メニューオプションを選択すると， 軌道プレビュースライダが表示されます．

   • このスライダで最後に計画された軌跡の 詳細なレビューが可能になります．

基本的な動作

1. Motion Planning パネルの Planning タブを選択してください．

2. Query セクション下の Select Goal State の部分を展開しください．

   • <random valid> を選択して Update をクリックします．
   • グラフィック・ウィンドウの ゴール・ポジションを見てください．

3. Plan をクリックすると， MoveIt! の動作計画ライブラリが生成した ロボットの動作計画を見ることができます．

   • 繰り返し表示を停止するために Displays -> Motion Planning -> Planned Path -> Loop Animation を無効にしてください．
   • 軌跡を表示するために Displays -> Motion Planning -> Planned Path -> Show Trail を有効にしてください．

4. Execute をクリックして 産業ロボットシミュレータで動作計画を実行します．

   • 複数色のシーン上のロボット表示が更新されて， ロボットが目標位置に「移動」したことを確認してください．

5. この手順を2〜5回ほど繰り返してください．

   • インタラクティブ・マーカを使用して 手動でロボットを目的の位置に移動してみる．
   • 名前付きの姿勢（ 例: "straight up" ）を使ってみる．

   

発展的な動作

1. 動作計画アルゴリズム
   • Context タブを選択して， 動作計画アルゴリズム（ "OMPL" の隣にあるドロップダウンボックス ） をいろいろ選択して試してみてください．
   • RRTkConfigDefault アルゴリズムは往々にして遥かに高速です．
2. 環境上の障害物
   • "Goal State" を調整して ロボットを障害物（例えばテーブル）に干渉させて見てください．
     • 干渉するリンクは赤色に表示変更されます．
     • 位置が到達不能なため解を見つけようとするとき， ロボットがさまざまな位置を探索して解を見つけようとするのが見られます．
     • "Context" タブで "Use Collision-Aware IK" 設定を無効にしてみてください．
     • 干渉がまだ検出されていてもソルバは干渉のないソリューションを探索しません．
   • 障害物を通る軌道を計画してみてください．
     • Goal State を動かす時に "Collision-Aware IK" が無効だと楽です．
     • ロボットの動作計画に失敗する場合は， エラーログを確認して計画要求を繰り返してください．
     • デフォルトのプランナはサンプリングベースなので， 実行ごとに異なる結果が生じることがあります．
     • 動作計画の作成に成功するように， 計画の計算時間を増やすこともできます．
     • より複雑な動作計画タスクでは， 異なる動作計画アルゴリズムを試してみてください．
   • シーンに新しい障害を追加してみてください．
     • "Scene Objects" タブ内に I-Beam.dae CAD モデルを追加します．
       • このファイルは industrial_training リポジトリの ~/industrial_training/exercises/3.4/I-Beam.dae にあります．
     • 操作ハンドルを使用して I-Beam を面白そうな位置に移動してください．
     • Publish Scene を押して， 更新された位置を MoveIt! に通知します．
     • 障害物の周辺における動作計画を行ってみてください．

セッション 4 - Descartes パッケージと認識

スライド

C++ を用いた動作計画

ロボットをプログラムから動かすための MoveIt! の C++ インタフェースについて見ていきます．

モチベーション

前回の演習では MoveIt! の作業セル設定を行い， 動作計画ツールを用いて RViz でロボットを少し動かしました． 今度は動作計画とその実行をプログラムコードから行ってみましょう．

この演習では自身のプログラムから MoveIt! ノードとやり取りするための 基本的な C++ インターフェイスを紹介します． MoveIt! を使用する方法はいろいろとありますが， シンプルなアプリケーションの場合はこれが最も分かりやすい方法です．

リファレンス

• Move Group Interface tutorial

追加情報とリソース

• MoveIt! Tutorials
• MoveIt! home-page

Scan-N-Plan アプリケーション: 演習問題

本演習の目標は myworkcell_core ノードを次のように変更することです．

1. 視覚ノードへのサービスコールによって通知された部品位置の中央に ロボットのツールフレームを移動します．

Scan-N-Plan アプリケーション: ガイダンス

1. myworkcell_node.cpp ファイルを編集します．

   1. #include <tf/tf.h> を追加して， フレーム座標変換／ユーティリティのための tf ライブラリにアクセスできるようにします．

      • 既にこれまでの演習で tf パッケージの依存関係を 追加していることを思い出してください．

   2. ScanNPlan クラスの start メソッドで LocalizePart サービスからの応答を用いて 新しい move_target 変数を初期化します．

      geometry_msgs::Pose move_target = srv.response.pose;
      

      • vision_node のサービスへの呼び出しの 後に このコードを配置してください．

2. MoveGroupInterface を用いて move_target 位置への移動の動作計画・実行を行います．

   1. MoveGroupInterface クラスを使用するには moveit_ros_planning_interface パッケージを myworkcell_core パッケージの依存関係として追加する必要があります．

      これまでの演習同様にパッケージの CMakeLists.txt（2行）と package.xml（1行）を変更して moveit_ros_planning_interface 依存関係を追加してください．

   2. 適切な「インクルード」参照を追加して MoveGroupInterface を使用できるようにします．

      #include <moveit/move_group_interface/move_group_interface.h>
      

   3. ScanNPlan クラスの start() メソッドで moveit::planning_interface::MoveGroupInterface オブジェクトを作成します．

      作業セル MoveIt! パッケージ（ "manipulator" ）の作成時に定義した プランニング・グループの名前を持つ単一のコンストラクタがあります．

      moveit::planning_interface::MoveGroupInterface move_group("manipulator");
      

   4. move_group オブジェクトの setPoseTarget 関数を用いて 直交座標系上の意図する目標位置を設定します．

      オブジェクトの move() 関数を呼び出して 目標位置への移動の動作計画を行い，実行します．

      // Plan for robot to move to part
      move_group.setPoseReferenceFrame(base_frame);
      move_group.setPoseTarget(move_target);
      move_group.move();
      

   5. move() コマンドのブロック中に ROS メッセージを処理できるようにするためには， ここ に記述があるように， move_group.move() コマンドに 「非同期の」スピナーを使用する必要があります．

      下記のように main() ルーチンのはじめのあたりで ros::init(argc, argv, "myworkcell_node") の後でスピナーを初期化し， 既存の ros::spin() コマンドを ros::waitForShutdown() に 置き換え ます．

      ros::AsyncSpinner async_spinner(1);
      async_spinner.start();
      ...
      ros::waitForShutdown();
      

3. システムをテストします．

   catkin build
   roslaunch myworkcell_moveit_config myworkcell_planning_execution.launch
   roslaunch myworkcell_support workcell.launch
   

4. より詳しく見ていきます．

   • RViz でトピック "/ar_pose_visual" の "Marker" 表示を追加して， 最終的なロボット位置が fake_ar_publisher によって発行された位置 と一致することを確認します．
   • 動作計画シーケンスを繰り返してみてください．
     1. MoveIt! RViz インタフェースを用いてアームを "allZeros" の位置に戻します．
     2. Ctrl+C で workcell.launch を停止した後に 再実行します．
   • workcell_node の start メソッドを更新して AR_target 位置に移動した後，自動的に allZeros 位置に戻るようにしてみてください．
     • move_group の利用可能なメソッドのリストについては ここ を参照してください．
   • 最終的なターゲットへの移動の前に， 目標位置から数インチ離れた「アプローチ位置」に移動してみてください．

直交座標系における軌道計画入門

モチベーション

MoveIt! はロボットを A 点から B 点に移動させることを目的とした， 「フリースペース」モーションを主な目的としたフレームワークで， それがどのように処理されているかについて ユーザは特に気にしなくても大丈夫なようになっています． このようなタイプの問題は実行されるタスクの多くあるケースの一部にすぎません． 溶接や塗装といった製造の「プロセス」を想像してみてください． ロボット動作の最初から最後まで ツールがどこを指しているのかについて非常に気を配っています．

このチュートリアルでは， 任意のプロセス経路に沿ってロボットを動かすための Descrates: 直交座標系（Cartesian/カーテジアン） モーション・プランナを紹介します． これはこの種の問題を解決する数多くの方法の1つではありますが， それはいくつかの巧妙な特性を持っています．

• 決定論的で，かつ全体としてみると（ある程度の検索解像度においては）最適です．
• 課題の冗長自由度を探索できます． （ 7つのロボット関節がある， もしくはツールのZ軸回転が問題ではないプロセスのような場合 ）

リファレンス

• Descartes Tutorial

追加情報とリソース

APIs:

• descartes_core::PathPlannerBase
• descartes_planner::DensePlanner
• descartes_planner::SparsePlanner

Scan-N-Plan アプリケーション: 演習問題

本演習では次のリファレンス・テンプレートに基づいて Scan-N-Plan アプリケーションに新しいノードを追加します．

1. マーカーの公称姿勢を ROS サービスを介して入力として受け取ります．
2. ロボットにマーカーの周囲をトレースするように命令する 関節軌道を生成します． （ 接着剤を塗布するような場合を想定 ）

Scan-N-Plan アプリケーション: ガイダンス

時間節約のために，次の内容の descartes_node.cpp というファイルを導入します．

1. 直交座標系における動作軌道計画のための 新しいノードと関連クラスを定義
2. 実際のサービスを定義し 直交座標系ライブラリを初期化
3. 高レベルの作業フローを提供（参照: planPath 関数）

演習課題として次のことを行います．

1. ロボット「軌道」を構成する 一連の直交座標系上の姿勢を定義します．
2. これらの軌道を Descartes が処理できるものに変換します．

ワークスペースのセットアップ

1. Descartes リポジトリを ワークスペースの src/ ディレクトリにクローンします．

   cd ~/catkin_ws/src
   git clone -b kinetic-devel https://github.com/ros-industrial-consortium/descartes.git
   

2. ur5_demo_descartes パッケージを ワークスペースの src/ ディレクトリにコピーします．

   cp -r ~/industrial_training/exercises/4.1/src/ur5_demo_descartes .
   

3. descartes_node_unfinished.cpp を コアパッケージの src/ フォルダにコピーして， descartes_node.cpp に名前を変更します．

   cp ~/industrial_training/exercises/4.1/src/descartes_node_unfinished.cpp myworkcell_core/src/descartes_node.cpp
   

4. これまでの演習と同じように下記のパッケージの依存関係を CMakeLists.txt と package.xml ファイルに記述します．

   • ur5_demo_descartes
   • descartes_trajectory
   • descartes_planner
   • descartes_utilities

5. descartes_node という名前の 新しいノードをビルドするために， myworkcell_core パッケージの CMakeLists.txt にルールを作成します．

   これまでの演習と同じように， これらの行をテンプレートファイルの類似した行の近くに追加します． （ 下は1つのブロックではありません ）

   add_executable(descartes_node src/descartes_node.cpp)
   add_dependencies(descartes_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
   target_link_libraries(descartes_node ${catkin_LIBRARIES})
   

Descartes ノードを完成させる

Descartes 動作計画アルゴリズムを実行するための サービス・インタフェースを作成します．

1. PlanCartesianPath.srv という名前の新しいサービスを myworkcell_core パッケージの srv/ ディレクトリに定義します．

   このサービスは目標物のセンタ位置を取得し， 目標物のエッジをトレースするための関節軌道を計算します．

   # request
   geometry_msgs/Pose pose
   
   ---
   
   # response
   trajectory_msgs/JointTrajectory trajectory
   

2. 新しく作成したサービスファイルを パッケージの CMakeLists.txt の add_service_file() ルールに追加します．

3. この新しいサービスは 他のパッケージのメッセージタイプを参照しているので， myworkcell_core の CMakeLists.txt（3行）と packagework.xml（1行）に依存関係として 他のパッケージ（ trajectory_msgs ）を追加する必要があります．

4. descartes_node.cpp を見返してコード構造を理解してください． 特に planPath メソッドは主要な手順の要点を記述しています．

5. Descartes ノード・ファイルにて TODO 指示を検索し， それらの部分を次のように拡張します．

   1. makeToolPoses では 目標物 "AR Marker" の外側をトレースする経路の 残りの3辺を生成します．
   2. makeDescartesTrajectory で， 作成した経路を直交座標系軌跡に1点1点変換します．
      • 公称点を指定された参照ポーズで変換することを忘れないでください． ref * point
   3. makeTolerancedCartesianPoint では， 与えられたポーズから new AxialSymmetricPt を作成します．
      • このポイント型の詳細については ここ を参照してください．
      • 90[deg]（ PI/2[rad] ）加算して， 点を Z 軸対称（ AxialSymmetricPt::Z_AXIS ）にします．

6. プロジェクトをビルドし， 新しい descartes_node にエラーがないことを確認してください．

作業セルノードの更新

Descartes ノードが完成した後， 新しい ServiceClient をメインの作業セルノードに追加して， そのロジックを呼び出す必要があります． このサービスの出力はこの後にロボットで実行する関節軌道です． ロボットでの実行は様々な方法で達成できますが， ここでは JointTrajectoryAction を直接呼びます．

1. myworkcell_node.cpp に 次のヘッダの include 文を追加します．

   #include <actionlib/client/simple_action_client.h>
   #include <control_msgs/FollowJointTrajectoryAction.h>
   #include <myworkcell_core/PlanCartesianPath.h>
   

   これらのライブラリとメッセージは MoveIt! から引かれているので 新しい依存関係を追加する必要はありません．

2. ScanNPlan クラスで 新しい PlanCartesianPath 変数の ServiceClient と FollowJointTrajectoryAction の アクションクライアントを追加します．

   ros::ServiceClient cartesian_client_;
   actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac_;
   

3. これらの新しいオブジェクトを コンストラクタで初期化します． アクションクライアントは initializer list と呼ばれるもので 初期化されなければならないことに注意してください．

   ScanNPlan(ros::NodeHandle& nh) : ac_("joint_trajectory_action", true)
   {
     // ... code
     cartesian_client_ = nh.serviceClient<myworkcell_core::PlanCartesianPath>("plan_path");
   }
   

4. start() 関数の最後で， 新しい直交座標系サービスを作成し， サービス要求を行います．

   // Plan cartesian path
   myworkcell_core::PlanCartesianPath cartesian_srv;
   cartesian_srv.request.pose = move_target;
   if (!cartesian_client_.call(cartesian_srv))
   {
     ROS_ERROR("Could not plan for path");
     return;
   }
   

5. 続けて次の行を追加して， （ MoveIt! をバイパスして）アクションサーバに 直接アクションを送信することで その軌道を実行します．

   // Execute descartes-planned path directly (bypassing MoveIt)
   ROS_INFO("Got cart path, executing");
   control_msgs::FollowJointTrajectoryGoal goal;
   goal.trajectory = cartesian_srv.response.trajectory;
   ac_.sendGoal(goal);
   ac_.waitForResult();
   ROS_INFO("Done");
   

6. プロジェクトをビルドし， 新しい descartes_node にエラーがないことを確認してください．

アプリケーション全体のテスト

1. workcell_node を除くすべてのノードを起動する 新しい setup.launch ファイルを workcell_support パッケージ内にを作成してください．

   <include file="$(find myworkcell_moveit_config)/launch/myworkcell_planning_execution.launch"/>
   <node name="fake_ar_publisher" pkg="fake_ar_publisher" type="fake_ar_publisher_node" />
   <node name="vision_node" type="vision_node" pkg="myworkcell_core" output="screen"/>
   <node name="descartes_node" type="descartes_node" pkg="myworkcell_core" output="screen"/>
   

2. 新しいセットアップファイルを実行してから メイン作業セルノードを実行します．

   roslaunch myworkcell_support setup.launch
   rosrun myworkcell_core myworkcell_node
   

   RViz で動作計画のループアニメーションが常に実行されていると 何が起きているのかを確認するのが難しくなります． RViz でこのループアニメーションを無効にし （ Displays -> Planned Path -> Loop Animation ） それから myworkcell_node を再実行します．

ヒントとヘルプ

• ヒント:
• makeToolPoses() で定義した軌道は 作業部分の既知の参照点からの相対的なものです． したがって (0, 0, 0) のツール姿勢は ワールド座標系の原点ではなく参照点そのものに位置します．
• makeDescartesTrajectorty(...) では "ref" 姿勢を使って 相対的なツール姿勢をワールド座標に変換する必要があります．
• makeTolerancedCartesianPoint(...) では， 一般的な関節軌道の特定の実装に関する 次のドキュメントを考慮してください．
  • http://docs.ros.org/indigo/api/descartes_trajectory/html/
• ヘルプ:
• さらに詳しいヘルプは 〜/industrial_training/exercises/4.1/src の完成コードを参考にしてください．

知覚処理入門

3D データの処理により慣れ親しむために， ASUS Xtion Pro（ または Microsoft Kinect ）センサーから 生成されたデータを使用してみます． そのデータストリームを見て RViz 内でで様々な方法でデータを視覚化します．

ポイントクラウド・データファイル

ほとんどの知覚処理はセンサからの ROS メッセージデータから始まります． 本演習では一般的な Kinect スタイルのセンサーの 3D Point Cloud データを使用します。

1. まずは RViz に表示するポイントクラウドのデータを ROS メッセージとしてパブリッシュします．

   1. ターミナルから roscore を開始します．

   2. 本演習用に新しいディレクトリを作成します．

      mkdir ~/ex4.2
      cd ~/ex4.2
      cp ~/industrial_training/exercises/4.2/table.pcd .
      

   3. 予め記録されたポイントクラウド・ファイル table.pcd から ポイントクラウド・メッセージをパブリッシュします．

      cd ~
      rosrun pcl_ros pcd_to_pointcloud table.pcd 0.1 _frame_id:=map cloud_pcd:=orig_cloud_pcd
      

   4. orig_cloud_pcd トピックが発行されていることを rostopic list で確認してください．

ポイントクラウドを RViz に表示する

1. ポイントクラウド処理の出力を表示するために RViz ウィンドウを起動します．

   rosrun rviz rviz
   

2. PointCloud2 ディスプレイ・アイテムを追加して 適切なトピック名を指定します．

   1. Displays パネル下部の Add をクリック
   2. PointCloud2 を選択
   3. ディスプレイツリーにある PointCloud2 を展開して topic ドロップダウンからトピックを選択
      • ヒント: ポイントクラウド・ファイルを使用している場合， トピック名は /orig_cloud_pcd です．

PCL を試す

次にポイントクラウドデータを処理するために PCL が提供するさまざまなコマンドラインツールを試します． 利用可能なコマンドラインツールは140個を超えていますが， 本演習ではいくつかのツールだけ使用します． その趣旨としてはコードを書くことなく PCL の機能に慣れることですが， これらのコマンドラインツールは自分でコードを書き始めるのに最適なものです． コマンドラインツールはさまざまな処理メソッドをテストするのに役立ちますが， ほとんどのアプリケーションでは通常は 実際の処理パイプラインに直接 C++ ライブラリを使用します． 発展的な ROS-I トレーニングコースでは， これらの C++ PCL メソッドを詳しく解説します．

下記の各 PCL コマンドは PCL 処理コマンドの結果から 新しいポイントクラウドファイル（ .pcd ）を生成します． pcl_viewer を使って出力を直接見るか， pcd_to_pointcloud コマンドを使ってポイントクラウドデータを RViz に表示するための ROS メッセージとしてパブリッシュします． RViz で出力を確認した後は pcd_to_pointcloud コマンドを停止してください．

pcl_voxel_grid を用いてポイントクラウドをダウンサンプリングする

1. グリッドサイズが (0.05, 0.05, 0.05) のボクセル・グリッドを使用して 元のポイントクラウドをダウンサンプリングします． ボクセルグリッドでは1つのグリッドキューブ内のすべての点が ボクセルの中心にある単一の点に置き換えられます． これは複雑すぎたり詳細すぎたりするセンサデータを単純化して， 処理ステップを高速化する一般的な方法です．

pcl_voxel_grid table.pcd table_downsampled.pcd -leaf 0.05,0.05,0.05
pcl_viewer table_downsampled.pcd

1. 新しいポイントクラウドを RViz で見てください．（任意）

rosrun pcl_ros pcd_to_pointcloud table_downsampled.pcd 0.1 _frame_id:=map cloud_pcd:=table_downsampled

注: RViz の PointCloud2 では トピックを /table_downsampled に変更して 新しいデータを表示してください．

pcl_sac_segmentation_plane を用いてポイントクラウドからテーブルサーフェスを抽出する

1. 最大の平面を見つけ， その平面に属する与えられた閾値内の点を抽出する．

pcl_sac_segmentation_plane table_downsampled.pcd only_table.pcd -thresh 0.01
pcl_viewer only_table.pcd

1. 新しいポイントクラウドを RViz で見てください．（任意）

rosrun pcl_ros pcd_to_pointcloud only_table.pcd 0.1 _frame_id:=map cloud_pcd:=only_table

注: RViz の PointCloud2 では トピックを /only_table に変更して 新しいデータを表示してください．

pcl_sac_segmentation_plane を用いてポイントクラウドからテーブル上で最大のクラスタを抽出する

1. テーブルに属する最大のポイントクラスタを抽出します．

pcl_sac_segmentation_plane table.pcd object_on_table.pcd -thresh 0.01 -neg 1
pcl_viewer object_on_table.pcd

1. 新しいポイントクラウドを RViz で見てください．（任意）

rosrun pcl_ros pcd_to_pointcloud object_on_table.pcd 0.1 _frame_id:=map cloud_pcd:=object_on_table

注: RViz の PointCloud2 では トピックを /object_on_table に変更して 新しいデータを表示してください．

pcl_outlier_removal を用いてポイントクラウドから異常値を削除する

1. 本例では異常値を除去するために統計的方法が使用されます． これはノイズの多いセンサデータをクリーンアップしたり， 追加処理する前に誤った中間生成物を除去したりするのに便利です．

pcl_outlier_removal table.pcd table_outlier_removal.pcd -method statistical
pcl_viewer table_outlier_removal.pcd

1. 新しいポイントクラウドを RViz で見てください．（任意）

rosrun pcl_ros pcd_to_pointcloud table_outlier_removal.pcd 0.1 _frame_id:=map cloud_pcd:=table_outlier_removal

注: RViz の PointCloud2 では トピックを /table_outlier_removal に変更して 新しいデータを表示してください．

pcl_normal_estimation を用いてポイントクラウドの各ポイントの法線を計算する

1. 本例では各点におけるローカルサーフェスの 法線（垂直）ベクトルを推定します． アルゴリズムは 各点について指定された半径内の近傍点を平面にフィットさせて 法線ベクトルを計算します． そして法線ベクトルについてより詳細に表示します。

pcl_normal_estimation only_table.pcd table_normals.pcd -radius 0.1
pcl_viewer table_normals.pcd -normals 10

マーチングキューブ法による再構成を用いてポイントクラウドをメッシュ化する

ポイントクラウドデータは構造化されていないことが多いのですが， 処理アルゴリズムがより構造化されたサーフェスメッシュに対して 処理する必要があることが時々あります． 本例ではマーチングキューブアルゴリズムを使用して， ポイントクラウドデータを近似したサーフェスメッシュを作成します．

pcl_marching_cubes_reconstruction table_normals.pcd table_mesh.vtk -grid_res 20
pcl_viewer table_mesh.vtk

応用デモ 1 - センサ認識を用いたマニュピレーション

Application Demo 1 - Perception-Driven Manipulation

イントロダクション - 知覚情報をもとに動作するマニピュレーション

目標

本演習はロボットが物を持ち上げて置く「ピック・アンド・プレース・タスク」 の一連の動作を行う ROS ノードを実行することを目標としています．加えて， これらの演習課題は ROS を基盤とした産業用アプリケーションに様々な機能 （認識・制御ドライバ・入出力・逆運動学・軌道計画・衝突回避）を統合する 用例となっています．

目的

• 実機またはシミュレーションのロボットアプリケーションの要素や構成の理解
• MoveIt! を用いたロボット動作の司令方法の学習
• 直交座標系上やリンク上の位置へのアーム移動の学習
• AR タグ (Augmented Reality Tag) 認識や PCL (Point Cloud Library) な どの認識機能の利用
• ピック・アンド・プレースのための衝突回避軌道計画
• グリッパのようなロボットの端部機器の制御

ピック・アンド・プレース演習パッケージ

本演習ではこれからの演習で利用する全ファイルを導入してその構成を習熟します．

ワークスペースの作成と初期化

cp -r ~/industrial_training/exercises/Perception-Driven_Manipulation/template_ws ~/perception_driven_ws
cd ~/perception_driven_ws
source /opt/ros/kinetic/setup.bash
catkin init

依存するソースファイルのダウンロード

wstool コマンドを用いて src/.rosinstall でリストアップされているリポジトリをダウンロードします．

cd ~/perception_driven_ws/src/
wstool update

依存する Debian パッケージのダウンロード

前提として rosdep tool がインストール されていることを確認してください．確認できましたらワークスペース内の src ディレクトリで下記コマンドを実行してください．

rosdep install --from-paths . --ignore-src -y

ワークスペースのビルド

catkin build

もしビルドが失敗するようでしたら前の2つのステップを再び行って依存関係にあるものがすべてダウンロードされていることを確認してください．

ワークスペース設定の反映

ワークスペースの大本のディレクトリにて下記コマンドを実行します．

source devel/setup.bash

演習用パッケージディレクトリ

cd ~/perception_driven_ws/src/collision_avoidance_pick_and_place/

launch ディレクトリの確認

ur5_setup.launch     : ROS システム全体の起動（MoveIt!，RViz， 認識機能，ROS-Iドライバ，ロボット入出力周辺機器）
ur5_pick_and_place.launch   : ピック・アンド・プレース・ノードの実行

config ディレクトリの確認

ur5/
 - pick_and_place_parameters.yaml    : ピック・アンド・プレース・ノードで読まれるパラメータのリスト
 - rviz_config.rviz   : RViz の表示プロパティの設定
 - target_recognition_parameters.yaml    : センサ情報から箱を検出する目標認識サービスのパラメータ
 - test_cloud_obstacle_descriptions.yaml    : センサ情報をシミュレートするときのパラメータ（シミュレーションセンサモードのみ）
 - collision_obstacles.txt   : シミュレートされたセンサ情報に加える各障害物ブロブの記述（シミュレーションセンサモードのみ）

src ディレクトリの確認

nodes :
 - pick_and_place_node.cpp : 全ての必要なヘッダや関数呼び出しを行うメインアプリケーションスレッド

tasks : 関数定義が不完全なソースファイルです．演習課題を完成させるために必要なコードを補完します．
 - create_motion_plan.cpp
 - create_pick_moves.cpp
 - create_place_moves.cpp
 - detect_box_pick.cpp
 - pickup_box.cpp
 - place_box.cpp
 - move_to_wait_position.cpp
 - set_attached_object.cpp
 - set_gripper.cpp

utilities:  
 - pick_and_place_utilities.cpp : 演習課題を完成するための補助関数

シミュレーションモードの開始

シミュレーションモードで動作するロボットの ROS システムを起動します．

セットアップ launch ファイルをシミュレーションモードで実行（ロボットとセンサともにシミュレーション）

ターミナルで下記のとおり実行します．

roslaunch collision_avoidance_pick_and_place ur5_setup.launch

RViz に初期位置にあるロボットを含む全ての作業セルコンポーネントが表示 されてシステムが準備された状態になります．ただしピック・アンド・プレー ス・ノードを実行するまでは何も動きません．

シミュレーションロボット＋実機センサのセットアップ

roslaunch collision_avoidance_pick_and_place ur5_setup.launch sim_sensor:=false

実機ロボット＋シミュレーションセンサのセットアップ

roslaunch collision_avoidance_pick_and_place ur5_setup.launch sim_robot:=false robot_ip:= [robot ip]

実機ロボット＋実機センサのセットアップ

roslaunch collision_avoidance_pick_and_place ur5_setup.launch sim_robot:=false robot_ip:= [robot ip] sim_sensor:=false sim_gripper:=false

初期化とグローバル変数

メインアプリケーション "pick_and_place_node.cpp" において，パブリック 変数にどのようなものがあるのか，アプリケーションが ROS ノードとしてど のように適切に初期化されているのかを見てみます．

アプリケーション変数

QT などで下記のファイルを開いてください．

[Source directory]/collision_avoidance_pick_and_place/include/collision_avoidance_pick_and_place/pick_and_place_utilities.h

C++ のクラス '''pick_and_place_config''' でパブリックグローバル変数が定義されていて，様々なプログラムから利用されます． それらの変数は下記のようになっています．

    ARM_GROUP_NAME  = "manipulator";
    TCP_LINK_NAME   = "tcp_frame";
    MARKER_TOPIC = "pick_and_place_marker";
    PLANNING_SCENE_TOPIC = "planning_scene";
    TARGET_RECOGNITION_SERVICE = "target_recognition";
    MOTION_PLAN_SERVICE = "plan_kinematic_path";
    WRIST_LINK_NAME = "ee_link";
    ATTACHED_OBJECT_LINK_NAME = "attached_object_link";
    WORLD_FRAME_ID  = "world_frame";
    HOME_POSE_NAME  = "home";
    WAIT_POSE_NAME  = "wait";
    AR_TAG_FRAME_ID    = "ar_frame";
    GRASP_ACTION_NAME = "grasp_execution_action";
    BOX_SIZE        = tf::Vector3(0.1f, 0.1f, 0.1f);
    BOX_PLACE_TF    = tf::Transform(tf::Quaternion::getIdentity(), tf::Vector3(-0.8f,-0.2f,BOX_SIZE.getZ()));
    TOUCH_LINKS = std::vector<std::string>();
    RETREAT_DISTANCE  = 0.05f;
    APPROACH_DISTANCE = 0.05f;

メインプログラム pick_and_place_node.cpp において，グローバルオブジェ クト application はメンバ cfg を通じてプログラムの変数へのアクセス を提供しています．例えば WORLD_FRAME_ID を利用したい場合は次のように 行います．

ROS_INFO_STREAM("world frame: " << application.cfg.WORLD_FRAME_ID)

ノードの初期化

pick_and_place_node.cpp ファイルにおいて main 関数が PickAndPlace アプリケーションクラスとその ROS と MoveIt! コンポーネ ントを初期化しています．

int main(int argc,char** argv)
{
  geometry_msgs::Pose box_pose;
  std::vector<geometry_msgs::Pose> pick_poses, place_poses;

  /* =========================================================================================*/
  /*    INITIALIZING ROS NODE
      Goal:
      - Observe all steps needed to properly initialize a ros node.
      - Look into the 'cfg' member of PickAndPlace to take notice of the parameters that
        are available for the rest of the program. */
  /* =========================================================================================*/

  // ros initialization
  ros::init(argc,argv,"pick_and_place_node");
  ros::NodeHandle nh;
  ros::AsyncSpinner spinner(2);
  spinner.start();

  // creating pick and place application instance
  PickAndPlace application;

  // reading parameters
  if(application.cfg.init())
  {
    ROS_INFO_STREAM("Parameters successfully read");
  }
  else
  {
    ROS_ERROR_STREAM("Parameters not found");
    return 0;
  }

  // marker publisher
  application.marker_publisher = nh.advertise<visualization_msgs::Marker>(
          application.cfg.MARKER_TOPIC,1);

  // planning scene publisher
  application.planning_scene_publisher = nh.advertise<moveit_msgs::PlanningScene>(
        application.cfg.PLANNING_SCENE_TOPIC,1);

  // MoveIt! interface
  application.move_group_ptr = MoveGroupPtr(
          new move_group_interface::MoveGroup(application.cfg.ARM_GROUP_NAME));

  // motion plan client
  application.motion_plan_client = nh.serviceClient<moveit_msgs::GetMotionPlan>(application.cfg.MOTION_PLAN_SERVICE);

  // transform listener
  application.transform_listener_ptr = TransformListenerPtr(new tf::TransformListener());

  // marker publisher (rviz visualization)
  application.marker_publisher = nh.advertise<visualization_msgs::Marker>(
          application.cfg.MARKER_TOPIC,1);

  // target recognition client (perception)
  application.target_recognition_client = nh.serviceClient<collision_avoidance_pick_and_place::GetTargetPose>(
          application.cfg.TARGET_RECOGNITION_SERVICE);

  // grasp action client (vacuum gripper)
  application.grasp_action_client_ptr = GraspActionClientPtr(
          new GraspActionClient(application.cfg.GRASP_ACTION_NAME,true));

アームの待機ポジションへの移動

MoveIt! の MoveGroup クラスではロボットを動作させるための様々な方法 が用意されています．MoveGroup により目標関節角度や直交座標系上の目標 位置，セットアップ・アシスタントにより予め定義された位置にロボットを動 かすことができます．本演習では予め定義された関節角度姿勢にロボットを動 かします．

関数の位置

• メインプログラム内の application.move_to_wait_position() という 関数を探してください．
• 関数の任意の部分をクリックし "F3" を押すことによってその関数のソー スファイルに移動します．
• もしくは [Source directory]/src/tasks/move_to_wait_position.cpp' ファイルを参照してください．
• ROS_ERROR STREAM ... を含む最初の行を削除してプログラムが実行さ れるようにしてください．

コードを完成させる

• 各箇所にある Fill Code: コメントを見つけてそこの説明を読んでくださ い．そして ENTER CODE HERE と書いてある各部分を適切なコードで書 き換えてください．

/* Fill Code:
     .
     .
     .
*/
/* ========  ENTER CODE HERE ======== */

• 初期化のときに予め定義された wait ポーズが cfg.WAIT_POSE_NAME に 保存されています．

コードのビルドと実行

• Qt でピック・アンド・プレース・ノードをコンパイルします．

Project -> Build Project

• もしくはターミナルで本演習のワークスペースディレクトリに移動して次のコマンドを実行します．

catkin build --cmake-args -G 'CodeBlocks - Unix Makefiles' --pkg collision_avoidance_pick_and_place
source ./devel/setup.bash

• launch ファイルでノードを実行します．

roslaunch collision_avoidance_pick_and_place ur5_pick_and_place.launch

• ロボットが待機ポジションにない場合には待機ボジションに移動します．ターミナルには次のようなメッセージが表示されるはずです．

[ INFO] [1400553673.460328538]: Move wait Succeeded
[ERROR] [1400553673.460434627]: set_gripper is not implemented yet.  Aborting.

API リファレンス

• setNamedTarget()
• move()

グリッパを開く

グリッパを開く目標を送るために "grasp action client" を利用することを 目的としています．

関数の位置

• メインプログラム内の application.set_gripper() という関数を探してください．
• 関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• ROS_ERROR STREAM ... を含む最初の行を削除してプログラムが実行されるようにしてください．

コードを完成させる

• 各箇所にある Fill Code: コメントを見つけてその記述を読んでください．そして ENTER CODE HERE と書いてある各部分を適切なコードで書き換えてください．

/* Fill Code:
     .
     .
     .
*/
/* ========  ENTER CODE HERE ======== */

• grasp_goal のプロパティは次の3つの値をとることができます．

    grasp_goal.goal = object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP;
    grasp_goal.goal = object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE;
    grasp_goal.goal = object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP;

• つかむフラグが設定できたら "grasp action client" に対して目標を送ることができます．

コードのビルドと実行

• Qt でピック・アンド・プレース・ノードをコンパイルします．

Project -> Build Project

• もしくはターミナルで本演習のワークスペースディレクトリに移動して次のコマンドを実行します．

catkin build collision_avoidance_pick_and_place

• launch ファイルでノードを実行します．

roslaunch collision_avoidance_pick_and_place ur5_pick_and_place.launch

• タスクが正常に実行された場合は次のようなメッセージが表示されるはずで す．ロボットは動きません，グリッパの I/O だけ作動します．

[ INFO] [1400553290.464877904]: Move wait Succeeded
[ INFO] [1400553290.720864559]: Gripper opened
[ERROR] [1400553290.720985315]: detect_box_pick is not implemented yet.  Aborting.

API リファレンス

• sendGoal()

箱をつかむ位置の検出

箱をつかむ座標フレームはセンサ情報処理から検出する ROS サービスから要 求できます．箱をつかむ姿勢取得のための ROS サービスを呼び出すサービス クライアントの利用法について学習します．

関数の位置

• メインプログラム内の application.detect_box_pick() という関数を探してください．
• 関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• ROS_ERROR STREAM ... を含む最初の行を削除してプログラムが実行されるようにしてください．

コードを完成させる

• 各箇所にある Fill Code: コメントを見つけてその記述を読んでくださ い．そして ENTER CODE HERE と書いてある各部分を適切なコードで書き 換えてください．

/* Fill Code:
     .
     .
     .
*/
/* ========  ENTER CODE HERE ======== */

• プログラム内の target_recognition_client オブジェクトは call() メソッドを用いて ROS サービスにリクエストを送ることができます．
• その ROS サービスはセンサ情報を処理してサービス構造体メンバ srv.response.target_pose 内の箱をつかむ姿勢を返す要求を受け取ります．

コードのビルドと実行

• Qt でピック・アンド・プレース・ノードをコンパイルします．

Project -> Build Project

• もしくはターミナルで本演習のワークスペースディレクトリに移動して次のコマンドを実行します．

catkin build --pkg collision_avoidance_pick_and_place

• launch ファイルでノードを実行します．

roslaunch collision_avoidance_pick_and_place ur5_pick_and_place.launch

• 1つの青い箱といくつかのボクセルグリッド障害物が RViz に表示され，ターミナルには次のようなメッセージが表示されるはずです．

[ INFO] [1400554224.057842127]: Move wait Succeeded
[ INFO] [1400554224.311158465]: Gripper opened
[ INFO] [1400554224.648747043]: target recognition succeeded
[ERROR] [1400554224.649055043]: create_pick_moves is not implemented yet.  Aborting.

API リファレンス

• call()

つかみ上げ動作の生成

グリッパは箱をつかみ上げるためのそれぞれの姿勢 : アプローチ・置く・後退 に動きます． 本演習ではそれらのツール中心位置 (Tool Center Point : TCP) 座標系におけるつかみ上げ動作を生成して，その後にそれらの姿勢を手首フレーム座標系に変換します．

関数の位置

• メインプログラム内の application.create_pick_moves() という関数を探してください．
• 関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• ROS_ERROR STREAM ... を含む最初の行を削除してプログラムが実行されるようにしてください．

コードを完成させる

• 各箇所にある Fill Code: コメントを見つけてその記述を読んでください．そして ENTER CODE HERE と書いてある各部分を適切なコードで書き換えてください．

/* Fill Code:
     .
     .
     .
*/
/* ========  ENTER CODE HERE ======== */

• create_manipulation_poses() は意図した目標にに対応するポーズを生成するためにアプローチと後退の距離を使用します．
• MoveIt! がロボットの手首の軌道を計算するので，全てのつかむための姿勢が手首フレームの座標系に変換される必要があります．
• lookupTransform メソッドが目標姿勢から他の姿勢に対する相対変換を行います．

コードのビルドと実行

• Qt でピック・アンド・プレース・ノードをコンパイルします．

Project -> Build Project

• もしくはターミナルで本演習のワークスペースディレクトリに移動して次のコマンドを実行します．

catkin build --cmake-args -G 'CodeBlocks - Unix Makefiles' --pkg collision_avoidance_pick_and_place
source ./devel/setup.bash

• Run your node with the launch file:
• launch ファイルでノードを実行します．

roslaunch collision_avoidance_pick_and_place ur5_pick_and_place.launch

• 次のようにツール中心姿勢とつかむ動作の手首位置がターミナルに表示されるはずです．

[ INFO] [1400555434.918332145]: Move wait Succeeded
[ INFO] [1400555435.172714267]: Gripper opened
[ INFO] [1400555435.424279410]: target recognition succeeded
[ INFO] [1400555435.424848964]: tcp position at pick: 0x7fffde492790
[ INFO] [1400555435.424912520]: tcp z direction at pick: 0x7fffde492950
[ INFO] [1400555435.424993675]: wrist position at pick: x: -0.81555
y: 0.215563
z: 0.3
[ERROR] [1400555435.425051853]: pickup_box is not implemented yet.  Aborting.

API リファレンス

• lookupTransform
• TF Transforms and other useful data types

箱をつかみ上げる

本演習では環境上の障害物を避けながら目標物をつかむようにロボットを動かします． 各姿勢の計画と吸引グリッパを適切に開閉することにより達成されます． また，MoveIt! における動作計画の作成方法も紹介します．

関数の位置

• メインプログラム内の application.pickup_box() という関数を探してください．
• 関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• ROS_ERROR STREAM ... を含む最初の行を削除してプログラムが実行されるようにしてください．

コードを完成させる

• 各箇所にある Fill Code: コメントを見つけてその記述を読んでください．そして ENTER CODE HERE と書いてある各部分を適切なコードで書き換えてください．

/* Fill Code:
     .
     .
     .
*/
/* ========  ENTER CODE HERE ======== */

• 動作計画を生成するのに用いる robot_state オブジェクトの取扱方法を理解するために set_atteched_object メソッドを調べてみてください．
• 全体の動作計画リクエストの定義方法と送られる方法を見るために create_motion_plan メソッドを調べてみてください．
• execute() メソッドが動作計画をロボットに送っています．

コードのビルドと実行

• Qt でピック・アンド・プレース・ノードをコンパイルします．

Project -> Build Project

• もしくはターミナルで本演習のワークスペースディレクトリに移動して次のコマンドを実行します．

catkin build --cmake-args -G 'CodeBlocks - Unix Makefiles' --pkg collision_avoidance_pick_and_place

• launch ファイルでノードを実行します．

roslaunch collision_avoidance_pick_and_place ur5_pick_and_place.launch

• 前回の演習に加えてロボットがつかむ動作群（アプローチ・つかみ・後退）を実行し，ターミナルには次のように表示されるはずです．

[ INFO] [1400555978.404435764]: Execution completed: SUCCEEDED
[ INFO] [1400555978.404919764]: Pick Move 2 Succeeded
[ERROR] [1400555978.405061541]: create_place_moves is not implemented yet.  Aborting.

API リファレンス

• Useful '''MoveGroup'''

置く動作の生成

グリッパは箱を置くためのそれぞれの姿勢 : アプローチ・置く・後退 に動きます． 本演習ではツール中心位置座標系における置く動作を生成して，その後それらの姿勢を手首フレーム座標系に変換します．

関数の位置

• メインプログラム内の application.create_place_moves() という関数を探してください．
• 関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• ROS_ERROR STREAM ... を含む最初の行を削除してプログラムが実行されるようにしてください．

コードを完成させる

• 各箇所にある Fill Code: コメントを見つけてその記述を読んでください．そして ENTER CODE HERE と書いてある各部分を適切なコードで書き換えてください．

/* Fill Code:
     .
     .
     .
*/
/* ========  ENTER CODE HERE ======== */

• 箱を置く位置はグローバル変数 cfg.BOX_PLACE_TF に保存されています．
• create_manipulation_poses() は意図した目標にに対応するポーズを生成するためにアプローチと後退の距離を使用します．
• MoveIt! がロボットの手首の軌道を計算するので，全ての置くための姿勢が手首フレームの座標系に変換される必要があります．
• lookupTransform メソッドが目標姿勢から他の姿勢に対する相対変換を行います．

コードのビルドと実行

• Qt でピック・アンド・プレース・ノードをコンパイルします．

Project -> Build Project

• もしくはターミナルで本演習のワークスペースディレクトリに移動して次のコマンドを実行します．

catkin build --cmake-args -G 'CodeBlocks - Unix Makefiles' --workspace collision_avoidance_pick_and_place

• launch ファイルでノードを実行します．

roslaunch collision_avoidance_pick_and_place ur5_pick_and_place.launch

• 次のように置く場所に対するツール中心姿勢と手首位置がターミナルに表示されるはずです．

[ INFO] [1400556479.404133995]: Execution completed: SUCCEEDED
[ INFO] [1400556479.404574973]: Pick Move 2 Succeeded
[ INFO] [1400556479.404866351]: tcp position at place: 0x7fff1055d800
[ INFO] [1400556479.404934796]: wrist position at place: x: -0.422
y: 0.6
z: 0.3
[ERROR] [1400556479.404981729]: place_box is not implemented yet.  Aborting.

API リファレンス

• lookupTransform
• TF Transforms and other useful data types

箱を置く

本演習では付加された荷物も含めて環境上の障害物を避けながら置くようにロボットを動かします． またタスクを完遂するためには吸引グリッパを適切な時に開閉する必要があります．

関数の位置

• メインプログラム内の application.place_box() という関数を探してください．
• 関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• ROS_ERROR STREAM ... を含む最初の行を削除してプログラムが実行されるようにしてください．

コードを完成させる

• 各箇所にある Fill Code: コメントを見つけてその記述を読んでください．そして ENTER CODE HERE と書いてある各部分を適切なコードで書き換えてください．

/* Fill Code:
     .
     .
     .
*/
/* ========  ENTER CODE HERE ======== */

• execute() メソッドが動作計画をロボットに送っています．

コードのビルドと実行

• Qt でピック・アンド・プレース・ノードをコンパイルします．

Project -> Build Project

• もしくはターミナルで本演習のワークスペースディレクトリに移動して次のコマンドを実行します．

catkin build --cmake-args -G 'CodeBlocks - Unix Makefiles' --pkg collision_avoidance_pick_and_place

• launch ファイルでノードを実行します．

roslaunch collision_avoidance_pick_and_place ur5_pick_and_place.launch

• この時点で演習が完成して，ロボットがピック・アンド・プレース動作を行い待機姿勢に戻るはずです．おめでとう！

API リファレンス

• Useful " MoveGroup " Methods

応用デモ 2 - Descartes パッケージによる動作計画と実行

Application Demo 2 - Descartes Planning and Execution

イントロダクション

目標

• 本アプリケーションは準拘束軌道点群からなるロボット軌道の動作計画と実行を行うための直交座標系ライブラリの様々なコンポーネントを使う方法を実演します．

目的

• 直交座標系ライブラリのワークフローに慣れる
• カスタム直交座標系ロボットモデルの読込み方法の習熟
• 6自由度のツール姿勢の準拘束軌道の作成方法の習熟
• 直交座標系のプランナでのロボット軌道を計画する
• 直交座標系軌道から動作実行のための MoveIt! メッセージへ変換する
• ロボットで動作軌道を実行する

アプリケーション構成

演習を完成するまでに利用する全てのパッケージとファイルについて見てゆきます．

ワークスペースのコピーと初期化

cd ~/industrial_training/exercises/Descartes_Planning_and_Execution
cp -r template_ws ~/descartes_ws
cd ~/descartes_ws
source /opt/ros/kinetic/setup.bash
catkin init

依存するソースのダウンロード

src/.rosinstall にリストアップされたリポジトリをダウンロードするために wstool コマンドを使います．

cd ~/descartes_ws/src/
wstool update

依存するDebian パッケージのダウンロード

前提として rosdep tool がインストールされていることを確認してください． 確認できましたらワークスペース内の src ディレクトリで下記コマンドを実行してください．

rosdep install --from-paths . --ignore-src -y

ワークスペースのビルド

catkin build

もしビルドが失敗するようでしたら前の2つのステップを再び行い，依存関係にあるものがすべてダウンロードされていることを確認してください．

ワークスペース設定の反映

ワークスペースの大本のディレクトリにて下記コマンドを実行します．

source devel/setup.bash

アプリケーションの全パッケージの確認

cd ~/descartes_ws/src
ls -la

• plan_and_run : plan_and_run アプリケーションのためのソースコードが含まれている．本パッケージにあるソースファイルを編集して演習課題を完成させます．
• ur5_demo_moveit_config : MoveIt! でロボット動作を計画・実行するための補助ファイルが含まれている．本パッケージは MoveIt! のセットアップアシスタントで生成されている．
• ur5_demo_support : ロボットが URDF ファイルとして定義されている．この URDF ファイルは plan_and_run アプリケーション実行時に読み込まれます．
• ur5_demo_descartes : UR5 アームのカスタム直交座標系ロボットモデルが用意されています．逆運動学の閉形式解を用いており，MoveitStateAdapter で使用されている数値解析的アプローチよりも著しく速く解を得られます．

plan_and_run パッケージ

roscd plan_and_run
ls -la

• src : アプリケーション・ソースファイル・ディレクトリ
• src/demo_application.cpp : アプリケーション実行するコードを含むクラスソースファイル
• src/plan_and_run.cpp : アプリケーションのメインアクセスファイル．全てのタスクを呼び出しmain ルーチンの中から順次実行します．
• src/tasks : 本演習を通して編集・完成させる全てのソースファイルが含まれているディレクトリ
• include : ヘッダファイルの・ディレクトリ
• include/plan_and_run/demo_application.h : アプリケーションの骨格の定義され，演習で使用されるグローバル変数が用意されている．
• launch: アプリケーションを実行するための launch ファイル
• launch/demo_setup.launch : roscore や moveit，アプリケーションで必要となるランタイム・リソースが読み込まれる．
• launch/demo_run.launch : ROS ノードとしてアプリケーションのメインプログラムを実行する．
• config: 重要性の低い設定ファイルのディレクトリ

メインアプリケーション・ソースファイル

"plan_and_run/src/plan_and_run_node.cpp" 内に下記のコードがあります．

int main(int argc,char** argv)
{
  ros::init(argc,argv,"plan_and_run");
  ros::AsyncSpinner spinner(2);
  spinner.start();

  // creating application
  plan_and_run::DemoApplication application;

  // loading parameters
  application.loadParameters();

  // initializing ros components
  application.initRos();

  // initializing descartes
  application.initDescartes();

  // moving to home position
  application.moveHome();

  // generating trajectory
  plan_and_run::DescartesTrajectory traj;
  application.generateTrajectory(traj);


  // planning robot path
  plan_and_run::DescartesTrajectory output_path;
  application.planPath(traj,output_path);

  // running robot path
  application.runPath(output_path);

  // exiting ros node
  spinner.stop();

  return 0;
}

このプログラムは application オブジェクトから各演習で対応する関数を呼び出して実行します． 例えばこのプログラムは直交座標系の定義を行うために application.iniDescartes() を呼び出します． つまり各々の機能が実装されるソースファイルを編集することで各演習を行っていき， この例では application.initDescartes（） の plan_and_run/src/tasks/init_descartes.src ソースファイルを編集します．

DemoApplication クラス

ヘッダファイル "plan_and_run/include/plan_and_run/demo_application.h" には補助構成要素とともにアプリケーションのメインクラスの定義があります．重要な構成要素のいくつかを下に書き出しています．

• Program Variables: Contain hard coded values that are used at various points in the application.
• プログラムの変数: アプリケーションの様々なところから利用されるハードコードされた値も含む

const std::string ROBOT_DESCRIPTION_PARAM = "robot_description";
const std::string EXECUTE_TRAJECTORY_SERVICE = "execute_kinematic_path";
const std::string VISUALIZE_TRAJECTORY_TOPIC = "visualize_trajectory_curve";
const double SERVICE_TIMEOUT = 5.0f; // seconds
const double ORIENTATION_INCREMENT = 0.5f;
const double EPSILON = 0.0001f;
const double AXIS_LINE_LENGHT = 0.01;
const double AXIS_LINE_WIDTH = 0.001;
const std::string PLANNER_ID = "RRTConnectkConfigDefault";
const std::string HOME_POSITION_NAME = "home";

• Trajectory 型: 直交座標系軌道点群の配列を表現するのに便利

typedef std::vector<descartes_core::TrajectoryPtPtr> DescartesTrajectory;

• DemoConfiguration Data Structure: 対応する ROS パラメータから実行時が初期化される変数を提供

struct DemoConfiguration
{
  std::string group_name;                 /* Name of the manipulation group containing the relevant links in the robot */
  std::string tip_link;                   /* Usually the last link in the kinematic chain of the robot */
  std::string base_link;                  /* The name of the base link of the robot */
  std::string world_frame;                /* The name of the world link in the URDF file */
  std::vector<std::string> joint_names;   /* A list with the names of the mobile joints in the robot */


  /* Trajectory Generation Members:
   *  Used to control the attributes (points, shape, size, etc) of the robot trajectory.
   *  */
  double time_delay;              /* Time step between consecutive points in the robot path */
  double foci_distance;           /* Controls the size of the curve */
  double radius;                  /* Controls the radius of the sphere on which the curve is projected */
  int num_points;                 /* Number of points per curve */
  int num_lemniscates;            /* Number of curves*/
  std::vector<double> center;     /* Location of the center of all the lemniscate curves */
  std::vector<double> seed_pose;  /* Joint values close to the desired start of the robot path */

  /*
   * Visualization Members
   * Used to control the attributes of the visualization artifacts
   */
  double min_point_distance;      /* Minimum distance between consecutive trajectory points. */
};

• DemoApplication Class: プログラム各ステップの関数を提供するアプリケーションの主要コンポーネント．このアプリケーションを ROS ノードに変換するいくつかの構成要素も含まれています．

class DemoApplication
{
public:
  /*  Constructor
   *    Creates an instance of the application class
   */
  DemoApplication();
  virtual ~DemoApplication();

  /* Main Application Functions
   *  Functions that allow carrying out the various steps needed to run a
   *  plan an run application.  All these functions will be invoked from within
   *  the main routine.
   */

  void loadParameters();
  void initRos();
  void initDescartes();
  void moveHome();
  void generateTrajectory(DescartesTrajectory& traj);
  void planPath(DescartesTrajectory& input_traj,DescartesTrajectory& output_path);
  void runPath(const DescartesTrajectory& path);

protected:

  /* Support methods
   *  Called from within the main application functions in order to perform convenient tasks.
   */

  static bool createLemniscateCurve(double foci_distance, double sphere_radius,
                                    int num_points, int num_lemniscates,
                                    const Eigen::Vector3d& sphere_center,
                                    EigenSTL::vector_Affine3d& poses);

  void fromDescartesToMoveitTrajectory(const DescartesTrajectory& in_traj,
                                              trajectory_msgs::JointTrajectory& out_traj);

  void publishPosesMarkers(const EigenSTL::vector_Affine3d& poses);


protected:

  /* Application Data
   *  Holds the data used by the various functions in the application.
   */
  DemoConfiguration config_;



  /* Application ROS Constructs
   *  Components needed to successfully run a ros-node and perform other important
   *  ros-related tasks
   */
  ros::NodeHandle nh_;                        /* Object used for creating and managing ros application resources*/
  ros::Publisher marker_publisher_;           /* Publishes visualization message to Rviz */
  ros::ServiceClient moveit_run_path_client_; /* Sends a robot trajectory to moveit for execution */



  /* Application Descartes Constructs
   *  Components accessing the path planning capabilities in the Descartes library
   */
  descartes_core::RobotModelPtr robot_model_ptr_; /* Performs tasks specific to the Robot
                                                     such IK, FK and collision detection*/
  descartes_planner::SparsePlanner planner_;      /* Plans a smooth robot path given a trajectory of points */

};

アプリケーション launch ファイル

アプリケーションを ROS ノードとして開始し，必要なパラメーターを ROS パラメーター・サーバーにロードします． "plan_and_run/launch/demo_run.launch" を見てこれがどのように行われているか確認してください．

<launch>
  <node name="plan_and_run_node" type="plan_and_run_node" pkg="plan_and_run" output="screen">
    <param name="group_name" value="manipulator"/>
    <param name="tip_link" value="tool"/>
    <param name="base_link" value="base_link"/>
    <param name="world_frame" value="world"/>
    <param name="trajectory/time_delay" value="0.1"/>
    <param name="trajectory/foci_distance" value="0.07"/>
    <param name="trajectory/radius" value="0.08"/>
    <param name="trajectory/num_points" value="200"/>
    <param name="trajectory/num_lemniscates" value="4"/>
    <rosparam param="trajectory/center">[0.36, 0.2, 0.1]</rosparam>
    <rosparam param="trajectory/seed_pose">[0.0, -1.03, 1.57 , -0.21, 0.0, 0.0]</rosparam>
    <param name="visualization/min_point_distance" value="0.02"/>
  </node>
</launch>

• 重要なパラメータ
• group_name: アームの運動学的なつながり（基礎部リンクからツール端部リンクまで）を含むリンクのリストを示す名前空間
• tip_link: 運動学的なつながりの最端部のリンク名（多くの場合はツールリンク）
• base_link: ロボットの基礎部分のリンク名
• world_frame: 動作計画空間上に定義された全ての物体が参照する絶対座標系フレーム
• "trajectory" 名前空間にあるパラメータは直交座標系プランナに供給される軌道の生成に用いられます．
• trajectory/seed_pose: 特に重要なパラメータで，軌道計画時に理想的な開 始と終了のロボット関節設定を示すために使われます．"seed_pose" が指定 されていない場合，複数の開始と終了の関節設定を考慮して，いくつかの開 始・終了の姿勢を組み合わせた複数の軌道解を得るために動作計画の時間が 長くなってしまいます．

全体の手順

演習を通してデモをどのように進めていくかについて提示します．また，シミュレーションモードや実機ロボットでどのようにしてシステムを実行するかについても見てゆきます．

主要目標

全体としては plan_and_run を段階的に実装してゆきます． つまり，デモアプリケーションを完成させるために必要な個々のピースを各演習で加えてゆきます． 各演習が完成したら，それらの結果を評価するためにシミュレーションでデモを実行してください． 全ての演習を終えた場合にのみ実機のロボットで成果を実行するようにしてください．

演習課題を完成させる

1. 演習課題を完成させるために src/plan_and_run/src/tasks/ ディレクトリの中にある対応したソースファイルを開いてください．例えば 演習1 では load_parameters.cpp を開きます．
2. 各々の演習を完成するための方法の具体的な指示がヘッダのコメントにありますのでそれを読んでください． 例としてファイル load_parameters.cpp には下記のとおり説明とヒントが書いてあります．

 /* LOAD PARAMETERS
   Goal:
     - Load missing application parameters into the node from the ros parameter server.
     - Use a private NodeHandle in order to load parameters defined in the node's namespace.
   Hints:
     - Look at how the 'config_' structure is used to save the parameters.
     - A private NodeHandle can be created by passing the "~" string to its constructor.
 */

1. 下記の行をコメントアウトしてください．

ROS_ERROR_STREAM("Task '"<<__FUNCTION__ <<"' is incomplete. Exiting"); exit(-1);

この行は大抵，各関数の先頭に書いてあります． このステップを省略すると，プログラムはこの行に到達した時点ですぐに終了してしまいます．

1. /* Fill Code: で始まるコメントブロックに進んでください．この部分は後続のコードが正しくないか，コメントアウトされているか，または不完全であることを示しています．Fill Code に続く指示を読んで記述されているようにタスクを完成させてください．指示書きのコメントブロックは次の例のように書いてあります．

  /*  Fill Code:
   * Goal:
   * - Create a private handle by passing the "~" string to its constructor
   * Hint:
   * - Replace the string in ph below with "~" to make it a private node.
   */

1. [ COMPLETE HERE ] とある部分が適切なコードエントリに置き換えられます．正しいコードエントリはプログラムの変数や文字列，定数だったりします．1つの例を下に記します．

ros::NodeHandle ph("[ COMPLETE HERE ]: ?? ");

この場合，正しくは文字列が "~" となりますので下記のように行が書き換わります．

ros::NodeHandle ph("~");

1. 各演習の全タスクができたら正しく完成したかを評価するためにデモを実行します．（実行方法は次の節を読んでください．）

シミュレーションモードでのデモの実行

1. ターミナルから次のように入力して実行します．

roslaunch plan_and_run demo_setup.launch

• シミュレーションロボットが準備された後に RViz が起動し，UR5 アームがホームポジションの状態で表示されます．また，ターミナルに次のようなメッセージが表示されるはずです．

      .
      .
      .
********************************************************
* MoveGroup using:
*     - CartesianPathService
*     - ExecutePathService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
*     - GetPlanningSceneService
*     - ExecutePathService
********************************************************

[ INFO] [1430359645.694537917]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1430359645.694700640]: MoveGroup context initialization complete

All is well! Everyone is happy! You can start planning now!

• この launch フィアルは1回だけ起動する必要があります．

1. もう1つのターミナルで次のアプリケーション launch ファイルを実行します．

roslaunch plan_and_run demo_run.launch

• RViz ウィンドウのアームが動作を開始します．

実機ロボットでのデモの実行

• 注意
  • ロボットに ping が通ることと，ロボット周辺に障害物がないことを確認してください．

1. ターミナルから次のように入力して実行します．

roslaunch plan_and_run demo_setup.launch sim:=false robot_ip:=000.000.0.00

注意:

• ロボットの実際の IP アドレスを引数 robot_ip に入力してください．RViz 内のロボットも実機ロボットと同じ姿勢に表示されているはずです．

1. もう1つのターミナルで次のアプリケーション launch ファイルを実行します．

roslaunch plan_and_run demo_run.launch

注意:

• 実機ロボットが動き始めます．

パラメータの読込み

プログラムで重要な変数を初期化するためにいくつかの ROS パラメータを読み込みます．

演習用ソースファイル

• plan_and_run/src/plan_and_run_node.cpp にあるメインアプリケーションのソースファイルを開きます．
• メインプログラム内の application.loadParameters() という関数を見てください．
• plan_and_run/src/tasks/load_parameters.cpp ファイルを開いて該当する関数を見てください． もしくは Qt/Eclipse で関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• //ROS_ERROR_STREAM("Task '"<<__FUNCTION__ <<"' is incomplete. Exiting"); exit(-1); を含む最初の行をコメントアウトしてプログラムが直ちに終了しないようにしてください．

コードを完成させる

• /* Fill Code: で始まるコメントブロックを見つけてそこにある記述どおりにコードを完成させてください．
• 指示に従って [ COMPLETE HERE ] とある各部分を書き換えてください．

コードのビルドと実行

• cd で本演習の catkin ワークスペースに移動して次を実行してビルドを行ってください．

catkin build --cmake-args -G 'CodeBlocks - Unix Makefiles'
source ./devel/setup.bash

• 次にアプリケーション launch ファイルを実行します．

roslaunch plan_and_run demo_setup.launch
roslaunch plan_and_run demo_run.launch

API リファレンス

• ros::NodeHandle
• NodeHandle::getParam()

ROS の初期化

MoveIt! とその他のシステムパートが通信できるように ROS コンポーネントの初期化を行います．

演習用ソースファイル

• plan_and_run/src/plan_and_run_node.cpp にあるメインアプリケーションのソースファイルを開きます．
• メインプログラム内の application.initRos() という関数を見てください．
• plan_and_run/src/tasks/init_ros.cpp ファイルを開いて該当する関数を見てください． もしくは Qt/Eclipse で関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• //ROS_ERROR_STREAM("Task '"<<__FUNCTION__ <<"' is incomplete. Exiting"); exit(-1); を含む最初の行をコメントアウトしてプログラムが直ちに終了しないようにしてください．

コードを完成させる

• ROS のパブリッシャ marker_publisher_ 変数がどのように初期化されるのかを確認してください．本プログラムのノードはそれを visualization_msgs::!MarkerArray メッセージを RViz 上に表示させるためにパブリッシュします．
• moveit_run_path_client_ サービスクライアントは ROS ノードハンドラのテンプレートメソッド ros::!NodeHandle::serviceClient() を利用します．
• /* Fill Code: で始まるコメントブロックを見つけてそこにある記述どおりにコードを完成させてください．
• 指示に従って [ COMPLETE HERE ] とある各部分を書き換えてください．

コードのビルドと実行

• cd で本演習の catkin ワークスペースに移動して catkin build でビルドしてください．
• 次にアプリケーション launch フィアルを実行してください．

roslaunch plan_and_run demo_run.launch

API リファレンス

• visualization_msgs::MarkerArray
• NodeHandle::serviceClient()

直交座標系の初期化

直交座標系ロボットモデルとツールの準拘束軌道を計画するために利用する軌道プランナのセットアップを行います．

演習用ソースファイル

• plan_and_run/src/plan_and_run_node.cpp にあるメインアプリケーションのソースファイルを開きます．
• メインプログラム内の application.initDescartes() という関数を見てください．
• plan_and_run/src/tasks/init_descartes.cpp ファイルを開いて該当する関数を見てください． もしくは Qt/Eclipse で関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• //ROS_ERROR_STREAM("Task '"<<__FUNCTION__ <<"' is incomplete. Exiting"); exit(-1); を含む最初の行をコメントアウトしてプログラムが直ちに終了しないようにしてください．

コードを完成させる

• 適切にロボットが初期化されるように descartes_core::RobotModel::initialize() メソッドを呼び出します．
• また robot_model_ 変数を descartes_core::!DensePlanner::initialize() にパスを通して直交座標系プランナを初期化します．
• /* Fill Code: で始まるコメントブロックを見つけてそこにある記述どおりにコードを完成させてください．
• 指示に従って [ COMPLETE HERE ] とある各部分を書き換えてください．

コードのビルドと実行

• cd で本演習の catkin ワークスペースに移動して catkin build でビルドしてください．
• 次にアプリケーション launch フィアルを実行してください．

roslaunch plan_and_run demo_run.launch

API リファレンス

• descartes_core::RobotModel
• descartes_planner::DensePlanner

準拘束軌道の生成

直交座標上の姿勢の配列から直交座標系軌道を生成します．各点はツールの Z 軸については回転自由とします．

演習用ソースファイル

• plan_and_run/src/plan_and_run_node.cpp にあるメインアプリケーションのソースファイルを開きます．
• メインプログラム内の application.generateTrajectory() という関数を見てください．
• plan_and_run/src/tasks/generate_trajectory.cpp ファイルを開いて該当する関数を見てください． もしくは Qt/Eclipse で関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• //ROS_ERROR_STREAM("Task '"<<__FUNCTION__ <<"' is incomplete. Exiting"); exit(-1); を含む最初の行をコメントアウトしてプログラムが直ちに終了しないようにしてください．

コードを完成させる

• ツールの軌道上の全てのポーズを生成するために createLemniscate() がどのように発動されているかを確認してください．
• Z 軸回りの回転自由な点を規定するために AxialSymmetric コンストラクタを使用してください．
• 列挙子 AxialSymmetricPt::FreeAxis::Z_AXIS は Z 軸回りに自由度を設定することが可能です．
• /* Fill Code: で始まるコメントブロックを見つけてそこにある記述どおりにコードを完成させてください．
• 指示に従って [ COMPLETE HERE ] とある各部分を書き換えてください．

コードのビルドと実行

• cd で本演習の catkin ワークスペースに移動して catkin build でビルドしてください．
• 次にアプリケーション launch フィアルを実行してください．

roslaunch plan_and_run demo_run.launch

API リファレンス

• descartes_trajectory::AxialSymmetricPt

ロボット軌道の計画

ロボット軌道計画を行うために前節の軌道を直交座標系プランナに送ります．

演習用ソースファイル

• plan_and_run/src/plan_and_run_node.cpp にあるメインアプリケーションのソースファイルを開きます．
• メインプログラム内の application.planPath() という関数を見てください．
• plan_and_run/src/tasks/plan_path.cpp ファイルを開いて該当する関数を見てください． もしくは Qt/Eclipse で関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• //ROS_ERROR_STREAM("Task '"<<__FUNCTION__ <<"' is incomplete. Exiting"); exit(-1); を含む最初の行をコメントアウトしてプログラムが直ちに終了しないようにしてください．

コードを完成させる

• 任意の関節ポーズに最も近いロボット関節の値を取得するために AxialSymmetricPt::getClosesJointPose() の利用のされかたを確認してください．また，ここでは複数の有効な関節構成ではなく，開始と終了などの1つの関節ポーズを選択することも可能です．
• 動作計画を算出するために DensePlanner::planPath() メソッドを呼び出します．
• 動作計画作成に成功したら DensePlanner::getPath() メソッドを使ってプランナから軌道を取得して output_path 変数に格納します．
• /* Fill Code: で始まるコメントブロックを見つけてそこにある記述どおりにコードを完成させてください．
• 指示に従って [ COMPLETE HERE ] とある各部分を書き換えてください

コードのビルドと実行

• cd で本演習の catkin ワークスペースに移動して catkin build でビルドしてください．
• 次にアプリケーション launch フィアルを実行してください．

roslaunch plan_and_run demo_run.launch

API リファレンス

• descartes_planner::DensePlanner

ロボット軌道の実行

動作計画で作成されたロボットの軌道を使ってロボットを動かします．

演習用ソースファイル

• plan_and_run/src/plan_and_run_node.cpp にあるメインアプリケーションのソースファイルを開きます．
• メインプログラム内の application.runPath() という関数を見てください．
• plan_and_run/src/tasks/run_path.cpp ファイルを開いて該当する関数を見てください． もしくは Qt/Eclipse で関数の任意の部分をクリックし "F3" を押すことによってその関数のソースファイルに移動します．
• //ROS_ERROR_STREAM("Task '"<<__FUNCTION__ <<"' is incomplete. Exiting"); exit(-1); を含む最初の行をコメントアウトしてプログラムが直ちに終了しないようにしてください．

コードを完成させる

• /* Fill Code: で始まるコメントブロックを見つけてそこにある記述どおりにコードを完成させてください．
• 指示に従って [ COMPLETE HERE ] とある各部分を書き換えてください

コードのビルドと実行

• cd で本演習の catkin ワークスペースに移動して catkin build でビルドしてください．
• 次にアプリケーション launch フィアルを実行してください．

roslaunch plan_and_run demo_run.launch

API リファレンス

• MoveGroupInterface::move()

応用編

セッション 5 - 軌道生成と知覚パイプラインの作成

スライド

高度な直交座標系動作軌道計画

直交座標系動作軌道プランナ Descrates を使って， ロボットがつかんでいる部品を固定工具で加工する複雑な動作軌道を生成します．

モチベーション

MoveIt! はロボットを A 点から B 点に移動させる 「フリースペース」モーションを主な目的としたフレームワークで， それがどのように処理されているかについて ユーザは特に気にしなくても大丈夫なようになっています． しかし，このようなタイプの問題は実行されるタスクの多くあるケースの一部にすぎません． 溶接や塗装といった製造の「プロセス」を想像してみてください． ロボット動作の最初から最後まで ツールがどこを指しているのかについて非常に気を配っています．

このチュートリアルでは， 任意のプロセス経路に沿ってロボットを動かすための Descrates: 直交座標系（Cartesian/カーテジアン） モーション・プランナを紹介します． これはこの種の問題を解決する数多くの方法の1つではありますが， いくつかの巧妙な特性を有しています．

• 決定論的で，かつ全体としてみると（ある程度の検索解像度においては）最適です．
• 課題の冗長自由度を探索できます． （ 7つのロボット関節がある， もしくはツールのZ軸回転が問題ではないプロセスのような場合 ）

リファレンス

• Descartes Tutorial

追加情報とリソース

APIs:

• descartes_core::PathPlannerBase
• descartes_planner::DensePlanner
• descartes_planner::SparsePlanner

Scan-N-Plan アプリケーション: 演習問題

本演習では新たに 2つのノード と 2つの xacro ，設定ファイル を Scan-N-Plan アプリケーションに付け加えます．

1. 軌道設定ファイル puzzle_bent.csv を使って ロボットが部品を持ち，固定ツールを周るマニピュレーション軌道を作成します．
2. ロボットにマーカーの周囲をトレースするように指示する関節軌道を生成します． （接着剤を塗布しているような例）

Scan-N-Plan アプリケーション: ガイダンス

時間節約のためにいくつかのファイルを導入します．

1. 1つ目は テンプレートノード adv_descartes_node.cpp で， 演習の大部分は形状の複雑な部品のバリ取りのための 複雑な軌道を作成するために費やされます．
2. 2つ目のノードは adv_myworkcell_node.cpp で， adv_descartes_node.cpp によって提供される adv_plan_path サービスを呼び出すように myworkcell_node.cpp を更新したファイルです．
3. 設定ファイル puzzle_bent.csv には 部品座標系に対する相対的な軌道が書かれています．
4. 2つの xacro ファイル puzzle_mount.xacroとgrinder.xacroは urdf/xacro の workcell.xacro ファイルを更新するものです．

演習では次のことを行います．

1. workcell.xacro ファイルを更新して， 2つの新しい xacro ファイルを組み込みます．
2. moveit_config パッケージを更新して， 新しいエンド・エフェクタ・リンクを含めた 本演習用の新しい Planning Group を定義します．
3. ロボットの「軌道」を構成する一連の直交座標系上の姿勢を定義します．
4. これらの軌道を Descrates ライブラリが理解できるものに変換します．

ワークスペースのセットアップ

1. 本演習では基本トレーニングコースと同じワークスペースを使用しますが， 演習 4.1 で完成させたワークスペースがない場合は， 完成したリファレンス・コードをコピーして， 次に示す他の依存関係のある必須パッケージを持ってきます． 既にワークスペースがある場合は次の手順に進んでください．

   mkdir ~/catkin_ws
   cd ~/catkin_ws
   cp -r ~/industrial_training/exercises/4.1/src .
   cd src
   git clone https://github.com/jmeyer1292/fake_ar_publisher.git
   git clone -b kinetic-devel https://github.com/ros-industrial-consortium/descartes.git
   sudo apt install ros-kinetic-ur-kinematics
   sudo apt install ros-kinetic-ur-description
   

2. adv_descartes_node_unfinished.cpp をコアパッケージの src/ フォルダにコピーして， ファイル名を adv_descartes_node.cpp に変更してください．

   cp ~/industrial_training/exercises/5.0/src/adv_descartes_node_unfinished.cpp myworkcell_core/src/adv_descartes_node.cpp
   

3. myworkcell_core パッケージの CMakeLists.txt にルールを作成して， 新しいノード adv_descartes_node をビルドします．

   先の演習と同様に，次の各行をひとまとまりのブロックとしてではなく テンプレートファイルの同じような行に追加します．

   add_executable(adv_descartes_node src/adv_descartes_node.cpp)
   add_dependencies(adv_descartes_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
   target_link_libraries(adv_descartes_node ${catkin_LIBRARIES})
   

4. adv_myworkcell_node.cpp をコアパッケージの src/ フォルダにコピーします．

   cp ~/industrial_training/exercises/5.0/src/myworkcell_core/src/adv_myworkcell_node.cpp myworkcell_core/src/
   

5. myworkcell_core パッケージの CMakeLists.txt にルールを作成して， 新しいノード adv_myworkcell_node をビルドします．

   先の演習と同様に，次の各行をひとまとまりのブロックとしてではなく テンプレートファイルの同じような行に追加します．

   add_executable(adv_myworkcell_node src/adv_myworkcell_node.cpp)
   add_dependencies(adv_myworkcell_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
   target_link_libraries(adv_myworkcell_node ${catkin_LIBRARIES})
   

6. 必要な設定ファイルをコピーします．

   mkdir ~/catkin_ws/src/myworkcell_core/config
   cp ~/industrial_training/exercises/5.0/src/myworkcell_core/config/puzzle_bent.csv myworkcell_core/config/
   cp ~/industrial_training/exercises/5.0/src/myworkcell_support/urdf/grinder.xacro myworkcell_support/urdf/
   cp ~/industrial_training/exercises/5.0/src/myworkcell_support/urdf/puzzle_mount.xacro myworkcell_support/urdf/
   mkdir ~/catkin_ws/src/myworkcell_support/meshes
   cp ~/industrial_training/exercises/5.0/src/myworkcell_support/meshes/* myworkcell_support/meshes/
   

7. 新しいパッケージの依存関係を追加します．

   • tf_conversions を CMakeLists.txt に2ヶ所， package.xml に1ヶ所，追加します．

workcell.xacro ファイルの更新

1. 新しく追加する grinder.xacro と puzzle_mount.xacro ファイルのための 2つの <include> タグを追加します．

2. 工具のグラインダを world リンクに次のオフセット値で付加します．

   <origin xyz="0.0 -0.4 0.6" rpy="0 3.14159 0"/>
   

   • grinder.xacro ファイルを見て 適切な <child_link> の名前を探します．
   • 他の <joint> タグ定義の1つをコピーして， 必要に応じて修正してください．

3. パズルマウントをロボットの tool0 フレームに次のオフセット値で付加します．

   <origin xyz="0 0 0" rpy="1.5708 0 0"/>
   

   • puzzle_mount.xacro ファイルを見て 適切な <child_link> の名前を探します． この部品の大本のリンクを見つけるには， 様々な <link> と <joint> の定義を調べる必要があります．
   • tool0 フレームはほとんどの ROS-I URDF で標準化された ロボットのエンド・エフェクタ取り付けフランジです．

4. moveit_config パッケージの demo.launch を起動して 作業セルを確認してください． グラインダがテーブルに立てられて， パズルの形状をした部品がロボットに付加されているはずです．

   roslaunch myworkcell_moveit_config demo.launch
   

新しい Planning Group の moveit_config パッケージへの追加

1. MoveIt! Setup Assistant を再び実行して， 新しい Planning Group として puzzle を作成します． base_link から part リンクまでの運動学的な連結を定義してください．

   roslaunch myworkcell_moveit_config setup_assistant.launch
   

   • 注: ジオメトリを追加したので 許される干渉の行列も再生成する必要があります．

高度な Descartes ノードを完成させる

1. まず最初に，関数 makePuzzleToolPoses() を完成させます． ファイル puzzle_bent.csv へのパスが必要です． コードの移植性を考慮してフルパスでハードコードすることは避けます． そのためには ROS のツール ros::package::getPath() を利用して， 関連パッケージへのルートパスを取得します．
   • 参考: getPath() API
2. 次に，関数 makeDescartesTrajectory() を完成させます． フレーム world と grinder_frame との間の変換が必要です． また各点は Z 軸から +/-PI に設定された方向許容誤差を持つ必要があります．
   • 参考
     • lookupTransform() API
     • tf::conversions 名前空間（Namespace）
     • TolerancedFrame の定義
     • OrientationTolerance の定義

setup.launch ファイルの更新

1. ファイルを更新します． adv というブール値の引数を取って， 基本的なノードか拡張された Descartes ノードの いずれかを起動できるようにします． どのノードを起動するかを制御するには <if> と <unless> 修飾子を使います．
   • 参考
     • roslaunch XML wiki
     • roslaunch XML wiki（日本語）

アプリケーション全体のテスト

1. セットアップファイルを実行してから， 拡張した作業セルノードを実行します．

   roslaunch myworkcell_support setup.launch adv:=true
   rosrun myworkcell_core adv_myworkcell_node
   

   • Descartes ノードは複雑な起動を計画するのに 何分かかかります ので，終了まで待ってください．
   • RViz の Planning Path の Loop Animation が 常に実行されいていると状況を確認しづらくなります． adv_myworkcell_node を実行する前に アニメーション・ループを RViz でオフにしてください．
     • Displays -> Planned Path -> Loop Animation

知覚パイプラインの構築

本演習では，知覚パイプラインを構築するための適切なコードを記入します． 最終目標は対象オブジェクトの姿勢情報を変換したものをブロードキャストすることです．

新しいワークスペースの準備

1. 本演習は以前の演習から独立した内容なので， 新しい catkin ワークスペースを作成します．

   1. gedit ~/.bashrc

   2. ~/catkin_ws/devel/setup.bash を実行している 最後の行をコメントアウト(#)してください．

      注釈

      新規にターミナルを開くたびに 新しい catkin ワークスペースのセットアップファイルを 手動で実行する必要が生じます．

   3. gedit を閉じて ROS 環境のセットアップを実行します．

      source /opt/ros/kinetic/setup.bash
      

2. テンプレート・ワークスペース・レイアウトとファイルのコピーを行います．

   cp -r ~/industrial_training/exercises/5.1/template_ws ~/perception_ws
   cd ~/perception_ws/
   

   1. 新しいワークスペースの初期化とビルドを行います．

      catkin init
      catkin build
      

   2. ワークスペース環境のセットアップを行います．

      source ~/perception_ws/devel/setup.bash
      

3. PointCloud ファイルを先の 演習 4.2 からホームディレクトリ (~) にコピーします．

   cp ~/industrial_training/exercises/4.2/table.pcd ~
   

4. QTCreator IDE に新しいワークスペースをインポートします．

   • QTCreator にて: File -> New File or Project -> Other Project -> ROS Workspace -> ~/perception_ws

イントロダクション（既存コードを読む）

ROS ノードの大半は既に完成していますので， 本演習の焦点は知覚のアルゴリズムとパイプラインです． CMakelists.txt と package.xml が完成しており，実行可能ファイルが提供されています． 実行可能ファイルはそのままでも実行できますがエラーが発生します． そこで提供されているソースコード perception_node.cpp を探して内容を見てください． 下記はコードの概要です．

1. ヘッダ:
   • PCL 関連のヘッダーのコメントを外す必要があります．
2. int main():
   • main 関数は while ループを含んだ状態で提供されています．
3. ROS の初期化:
   • ros::init と ros::NodeHandle の2つが呼び出され初期化されています． さらにノードの名前空間内に launch ファイルからパラメータを取得する場合に使用する プライベートノードハンドルがあります．
4. パラメータのセットアップ:
   • 現状，本例には3つの文字列パラメータがあります: ワールドフレーム，カメラフレーム および カメラが公開しているトピック です． これらのパラメータを launch ファイルから読み込む いくつかの nh.getParam 行を書くのは簡単だと思います． PCL メソッドのためにパラメータが多くなるため， ハードコードするよりも launch ファイルを使って調整するほうが便利です． 時間があればこれを設定しておいた方が良いでしょう．
5. パブリッシャのセットアップ:
   • 2つのパブリッシャがポイントクラウドの ROS メッセージを公開するように設定されています． 画像やポイントクラウドの処理では結果を視覚化すると便利なことがよくあります．
6. PointCloud2 のリスナ（ while ループ内 ）:
   • 通常は ここ （ 日本語版 ）で行われているように ROS の購読メソッドとコールバック関数を使用して ROS メッセージを待ち受けます． しかしこれをコールバック関数の外で行うと便利なことが多いので ``ros::topic::waitForMessage `` を使ってメッセージを聞く例を示します．
7. PointCloud2 の変換（ while ループ内 ）:
   • カメラフレーム上で作業することも可能ですが， XYZ 空間上でポイントクラウドの点を見た方がより理解しやすく便利です． そのために点群をカメラフレームからワールドフレームに変換しています．
8. PointCloud2 の変換（ ROS から PCL に / while ループ内 ）:
9. PointCloud2 の変換（ PCL から ROS に / while ループ内 ）:
   • このステップは必須ではありませんが， ポイントクラウドの処理結果を視覚化すると便利ですので ROS の型に戻して，パブリッシュ用の ROS メッセージを作成しています．

このように多くのことが既に行われているので， コードの仕上げは簡単かと思います． 必要があるのは途中の部分に書き入れることだけです．

主要なタスク: 空欄を埋める

知覚アルゴリズムを含む途中の部分を埋める作業は反復的なプロセスですので， 各ステップはそれぞれのサブタスクに分割されています．

Voxel フィルタの実装

1. ファイルの最上部付近にある voxel_grid のインクルード・ヘッダのコメントを外します．

2. コードの変更:

   ほとんどのポイントクラウド処理パイプラインの最初のステップは Voxel フィルタです． このフィルタは点群をダウンサンプリングするだけでなく， NAN 値を排除してより先のフィルタリングや処理が 実際的な値に対して行われるようにします． PCL Voxel Filter Tutorial を参照してください． もしくは下記のコードをコピーしてください．

   perception_node.cpp 内の次の部分を探してください．

   /* ========================================
    * Fill Code: VOXEL GRID
    * ========================================*/
   

   このバナーの下に次のコードをコピー＆ペーストしてください．

   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ> (cloud));
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
   pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
   voxel_filter.setInputCloud (cloud_ptr);
   voxel_filter.setLeafSize (float(0.002), float(0.002), float(0.002));
   voxel_filter.filter (*cloud_voxel_filtered);
   

3. perception_node.cpp 内のパブリッシャを更新します．

   次の部分を探してください．

   /* ========================================
    * CONVERT POINTCLOUD PCL->ROS
    * PUBLISH CLOUD
    * Fill Code: UPDATE AS NECESSARY
    * ========================================*/
   

   pcl::toROSMsg のコメントを外して *cloud_ptr を *cloud_voxel_filtered で置き換えます．

   *各新規アップデート後に RViz で表示するために

   パブリッシュされているポイントクラウドを置き換えます．*

   注釈

   時間と忍耐力があれば， フィルタの種類ごとに ROS パブリッシャを作成することをお勧めします． Rviz で異なるポイントクラウドを切り替えて 複数のフィルタの結果を一度に見ることができると便利です．

4. コンパイル

   catkin build
   

結果の表示

1. 知覚パイプラインを実行します．

   注釈

   RViz で Global Frame を kinect_link に変更してください．

   cd ~
   roscore
   rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 world_frame kinect_link
   rosrun pcl_ros pcd_to_pointcloud table.pcd 0.1 _frame_id:=kinect_link cloud_pcd:=kinect/depth_registered/points
   rosrun rviz rviz
   rosrun lesson_perception perception_node
   

2. 結果を見る

   RViz 内で PointCloud2 ディスプレイを追加してください． トピックには "object_cluster" を設定します． 表示されるのはオリジナルのポイントクラウド （ 演習 4.2を完了して新しいデフォルト設定を保存したか，その演習の設定を保存した場合 ） にオーバーレイされた Voxel フィルタの結果です．

   

3. 結果を表示し終えたら Voxel のフィルターサイズを 0.002 から 0.100 に変更して， 結果をもう一度表示してみてください． 終えたらフィルターを 0.002 に戻してください．

   • この変更の結果を確認するには Ctrl+C キーを使用して知覚ノードを終了し， リビルドしてから知覚ノードを再実行します．

   注釈

   他のノード（ RViz や ROS など ）を停止する必要はありません．

4. Voxel フィルタを確認できたら，Ctrl+C を入力して知覚ノードを停止してください．

パススルーフィルタの実装

1. 前回と同じようにファイルの最上部付近の PassThrough フィルタのインクルード・ヘッダのコメントアウトを外します．

2. コードの変更:

   次の関心のある領域を取得するために便利なフィルタリングは一連のパススルーフィルタです． これらのフィルタはポイントクラウドをある空間の容積まで縮小します． （ x y と z フィルタを使用する場合 ） ここでは x, y, z の各方向にそれぞれのパススルーフィルタを適用していきます． ヒントは PCL Pass-Through Filter Tutorial を参照するか，次のコードを使用してください．

   perception_node.cpp 内の次のセクションを探してください．

   /* ========================================
    * Fill Code: PASSTHROUGH FILTER(S)
    * ========================================*/
   

   このバナーの下に次のコードをコピー＆ペーストしてください．

   pcl::PointCloud<pcl::PointXYZ> xf_cloud, yf_cloud, zf_cloud;
   pcl::PassThrough<pcl::PointXYZ> pass_x;
   pass_x.setInputCloud(cloud_voxel_filtered);
   pass_x.setFilterFieldName("x");
   pass_x.setFilterLimits(-1.0,1.0);
   pass_x.filter(xf_cloud);
   
   pcl::PointCloud<pcl::PointXYZ>::Ptr xf_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(xf_cloud));
   pcl::PassThrough<pcl::PointXYZ> pass_y;
   pass_y.setInputCloud(xf_cloud_ptr);
   pass_y.setFilterFieldName("y");
   pass_y.setFilterLimits(-1.0, 1.0);
   pass_y.filter(yf_cloud);
   
   pcl::PointCloud<pcl::PointXYZ>::Ptr yf_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(yf_cloud));
   pcl::PassThrough<pcl::PointXYZ> pass_z;
   pass_z.setInputCloud(yf_cloud_ptr);
   pass_z.setFilterFieldName("z");
   pass_z.setFilterLimits(-1.0, 1.0);
   pass_z.filter(zf_cloud);
   

   フィルタの閾値を変更すると異なる結果を見ることができます．

3. pc2_cloud が入力されている pcl::toROSMsg 呼び出し部分を探してください． これは RViz ディスプレイで閲覧されるポイントクラウドです． 現在のクラウド（ *cloud_voxel_filter ）を 最終的なパススルーフィルタ結果（ zf_cloud ）に置き換えます．

4. コンパイルと実行

   catkin build
   rosrun lesson_perception perception_node
   

5. 結果の表示

   RViz 内でオリジナル・カメラデータの /kinect/depth_registered/points に基づいた PointCloud2 と 最新の処理結果の object_cluster トピックを比べてみてください． 元のポイントクラウドの一部が「切り取られて」いることが分かると思います．

   

   注釈

   X/Y/Z のフィルタ閾値を変更（ 例: +/- 0.5 ）してみてください． リビルドと実行し直して RViz で効果を見ます． いろいろ試し終わりましたら閾値を +/- 1.0 に戻してください．

6. パススルー・フィルタの挙動を確認できたら，Ctrl+C キーを使用してノードを停止してください． 他のターミナルを閉じたり他のノードを停止したりする必要はありません．

平面のセグメンテーション

1. コードの変更

   この方法は対象物が平坦な表面上にあるあらゆるアプリケーションにとって最も有用な方法の1つです． テーブル上のオブジェクトを分離するには， ポイント群を平面にフィットさせ， テーブルを構成するポイント群を見つけ， それらのポイント群を除外して， テーブル上のオブジェクトに対応するポイント群だけを残すようにします． これは私たちが使用する最も複雑な PCL メソッドで， 実際には RANSAC セグメンテーションモデルと 抽出インデックスツールの2つの組み合わせとなります． 詳細な例は PCL Plane Model Segmentation Tutorial にあります． もしくは下記コードをコピーして使用してください．

   perception_node.cpp 内の次のセクションを探してください．

   /* ========================================
    * Fill Code: PLANE SEGEMENTATION
    * ========================================*/
   

   このバナーの下に次のコードをコピー＆ペーストしてください．

   pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>(zf_cloud));
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
   // Create the segmentation object for the planar model and set all the parameters
   pcl::SACSegmentation<pcl::PointXYZ> seg;
   pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
   pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
   seg.setOptimizeCoefficients (true);
   seg.setModelType (pcl::SACMODEL_PLANE);
   seg.setMethodType (pcl::SAC_RANSAC);
   seg.setMaxIterations (200);
   seg.setDistanceThreshold (0.004);
   // Segment the largest planar component from the cropped cloud
   seg.setInputCloud (cropped_cloud);
   seg.segment (*inliers, *coefficients);
   if (inliers->indices.size () == 0)
   {
     ROS_WARN_STREAM ("Could not estimate a planar model for the given dataset.") ;
     //break;
   }
   

   平面モデルに合致した点を取得したら， 平面を構成するポイント群のポイントクラウドデータ構造内の インデックスを抽出することができます．

   // Extract the planar inliers from the input cloud
   pcl::ExtractIndices<pcl::PointXYZ> extract;
   extract.setInputCloud (cropped_cloud);
   extract.setIndices(inliers);
   extract.setNegative (false);
   
   // Get the points associated with the planar surface
   extract.filter (*cloud_plane);
   ROS_INFO_STREAM("PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." );
   

   もちろん，これらの点をクラウドから減算またはフィルタリングして， 平面上の点のみを取得することもできます．

   // Remove the planar inliers, extract the rest
   extract.setNegative (true);
   extract.filter (*cloud_f);
   

2. pc2_cloud が入力されている pcl::toROSMsg 呼び出し部分を探してください． これは RViz ディスプレイで閲覧されるポイントクラウドです． 現在のクラウド（ zf_cloud ）を 平面適合から外れた結果（ *cloud_f ）に置き換えます．

3. 前のステップと同様にコンパイルと実行を行ってください． このステップで必要なヘッダ部分のコメントアウトを外すのを忘れないようにしてください．

4. 結果の確認

   RViz 内でオリジナル・カメラデータの /kinect/depth_registered/points に基づいた PointCloud2 と 最新の処理結果の object_cluster トピックを比べてみてください． テーブル平面の上にある点だけが最終的な処理の結果として残っていることが分かると思います．

   

5. 結果の表示が終了したら，"setMaxIterations" および "setDistanceThreshold" の値を変更して， 平面適合点／不適合点としてデータをどれくらい厳密に制御できるかを試して， その結果を表示してみてください． MaxIterations = 100 と DistanceThreshold = 0.010 の値を試してみてください．

6. 平面分割の結果を確認できたら，Ctrl+C を入力してノードを停止してください． 他のターミナルを閉じたり他のノードを停止したりする必要はありません．

ユークリッドクラスタの抽出（任意／ただし推奨）

1. コードの変更

   この方法は複数のオブジェクトがあるすべてのアプリケーションにおいて有用です． またこれは複雑な PCL メソッドでもあります． 詳細な例は PCL Euclidean Cluster Extration Tutorial にあります．

   perception_node.cpp 内の次のセクションを探してください．

   /* ========================================
    * Fill Code: EUCLIDEAN CLUSTER EXTRACTION (OPTIONAL/RECOMMENDED)
    * ========================================*/
   

   PCL チュートリアルに従い，このセクションに次のコードを挿入します． このバナーの下に次のコードをコピー＆ペーストしてください．

   // Creating the KdTree object for the search method of the extraction
   pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
   *cloud_filtered = *cloud_f;
   tree->setInputCloud (cloud_filtered);
   
   std::vector<pcl::PointIndices> cluster_indices;
   pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
   ec.setClusterTolerance (0.01); // 2cm
   ec.setMinClusterSize (300);
   ec.setMaxClusterSize (10000);
   ec.setSearchMethod (tree);
   ec.setInputCloud (cloud_filtered);
   ec.extract (cluster_indices);
   
   std::vector<sensor_msgs::PointCloud2::Ptr> pc2_clusters;
   std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > clusters;
   for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
   {
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
     for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
       cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
     cloud_cluster->width = cloud_cluster->points.size ();
     cloud_cluster->height = 1;
     cloud_cluster->is_dense = true;
     std::cout << "Cluster has " << cloud_cluster->points.size() << " points.\n";
     clusters.push_back(cloud_cluster);
     sensor_msgs::PointCloud2::Ptr tempROSMsg(new sensor_msgs::PointCloud2);
     pcl::toROSMsg(*cloud_cluster, *tempROSMsg);
     pc2_clusters.push_back(tempROSMsg);
   }
   

2. pc2_cloud が入力されている pcl::toROSMsg 呼び出し部分を探してください． これは RViz ディスプレイで閲覧されるポイントクラウドです． 現在のクラウド（ *cloud_f ）を 最も大きいクラスタ（ *(clusters.at(0)) ）に置き換えます．

3. 前のステップと同様にコンパイルと実行を行ってください．

4. 結果を RViz で見ます． setClusterTolerance や setMinClusterSize， setMaxClusterSize パラメタをいじって効果を RViz 上で観察してみてください．parameters,

   

5. クラスタ抽出の結果を確認できたら，Ctrl+C を入力してノードを停止してください． 他のターミナルを閉じたり他のノードを停止したりする必要はありません．

クロップボックス・フィルタの作成

1. コードの変更

   この方法は本演習の2つ目のサブタスクだったパススルーフィルタに似ていますが， 一連の3つのパススルーフィルタを使う代わりに 1つのクロップボックス（ CropBox ）フィルタの適用で済みます．

   クロップボックス・フィルタのドキュメントと必要なヘッダファイルのの情報は Point Cloud Library (PCL) にあります．

   perception_node.cpp 内の次のセクションを探してください．

   /* ========================================
    * Fill Code: CROPBOX (OPTIONAL)
    * Instead of three passthrough filters, the cropbox filter can be used
    * The user should choose one or the other method
    * ========================================*/
   

   パススルー・フィルタをクロップボックス・フィルタで置き換えます． パススルー・フィルタは削除するかコメントアウトしてください．

   参考になる PCL のチュートリアルはなく，上記のリンクにある PCL ドキュメントしかありません． 一般的な設定方法は同じです． （ 結果出力の設定，フィルタ・インスタンスの宣言，入力の設定，パラメーターの設定，フィルター ）

   出力するポイントクラウドの設定:

   pcl::PointCloud<pcl::PointXYZ> xyz_filtered_cloud;
   

   フィルタ・インスタンスの宣言:

   pcl::CropBox<pcl::PointXYZ> crop;
   

   入力の設定:

   crop.setInputCloud(cloud_voxel_filtered);
   

   パラメータの設定:

   ドキュメントによると CropBox は値の最大値・最小値の入力として Eigen Vector4f 型をとります．

   Eigen::Vector4f min_point = Eigen::Vector4f(-1.0, -1.0, -1.0, 0);
   Eigen::Vector4f max_point = Eigen::Vector4f(1.0, 1.0, 1.0, 0);
   crop.setMin(min_point);
   crop.setMax(max_point);
   

   フィルタ:

   crop.filter(xyz_filtered_cloud);
   

   パススルーフィルタを削除またはコメントアウトして 既に平面セグメンテーションコードを記述している場合は， 平面セグメンテーションに渡すクラウドの名前を必ず更新してください． zf_cloud を xyz_filtered_cloud に置き換えます．

   pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>(xyz_filtered_cloud));
   

2. pc2_cloud が入力されている pcl::toROSMsg 呼び出し部分を探してください． これは RViz ディスプレイで閲覧されるポイントクラウドです． 現在のクラウドを 新しいフィルタ結果の（ xyz_filtered_cloud ）に置き換えます．

3. 前のステップと同様にコンパイルと実行を行ってください．

   次の現在使用している CropBox フィルタの画像は 平面セグメンテーション・フィルタの画像によく似ています．

   

統計的な外れ値の除去

1. コードの変更

   この方法は最終結果に複雑性や情報を必ずしも付加するとは限りませんが， しばしば有用なことがあります．

   チュートリアルは Removing outliers using a StatisticalOutlierRemoval filter にあります．

   perception_node.cpp 内の次のセクションを探してください．

   /* ========================================
    * Fill Code: STATISTICAL OUTLIER REMOVAL (OPTIONAL)
    * ========================================*/
   

   一般的な設定方法は同じです． （ 結果出力の設定，フィルタ・インスタンスの宣言，入力の設定，パラメーターの設定，フィルター ）

   出力するポイントクラウドの設定:

   pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud_ptr= clusters.at(0);
   pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
   

   フィルタ・インスタンスの宣言:

   pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
   

   入力の設定:

   sor.setInputCloud (cluster_cloud_ptr);
   

   パラメータの設定:

   ドキュメントを見ると，StatisticalOutlierRemoval は 検査する近傍の点の数と外れ値の除外に使用する標準偏差の閾値を使用します．

   sor.setMeanK (50);
   sor.setStddevMulThresh (1.0);
   

   フィルタ:

   sor.filter (*sor_cloud_filtered);
   

2. pc2_cloud が入力されている pcl::toROSMsg 呼び出し部分を探してください． 現在のクラウドを 新しいフィルタ結果の（ *sor_cloud_filtered ）に置き換えます．

3. 前のステップと同様にコンパイルと実行を行ってください．

   

TF のブロードキャストの作成

これはフィルタ・メソッドではありませんが， 処理パイプラインの結果を他のノードが使用できるようにパブリッシュする方法を提示しています． 多くの場合，処理パイプラインの目的は他のノードが使用するための測定値や 位置の定位，またはその他のメッセージを生成することです． 本サブタスクは TF 変換をブロードキャストして， テーブル上の最も大きい箱の位置を定義します． この変換は他のノードが把持するために箱の位置と向きを識別するのに使用されます．

1. コードの変更・挿入

   perception_node.cpp 内の次のセクションを探してください．

   /* ========================================
    * BROADCAST TRANSFORM (OPTIONAL)
    * ========================================*/
   

   ROS Wiki - Writing a tf broadcaster (C++) （ ROS Wiki 日本語 - tf の broadcaster を書く(C++) ） に沿って行っていきます． 重要な変更は位置と向きの情報 （ setOrigin(tf::Vector3(msg->x, msg->y, 0.0) と setRotation(q) ） の設定です．

   変換を作成:

   static tf::TransformBroadcaster br;
   tf::Transform part_transform;
   
   //Here in the tf::Vector3(x,y,z) x,y, and z should be calculated based on the pointcloud filtering results
   part_transform.setOrigin( tf::Vector3(sor_cloud_filtered->at(1).x, sor_cloud_filtered->at(1).y, sor_cloud_filtered->at(1).z) );
   tf::Quaternion q;
   q.setRPY(0, 0, 0);
   part_transform.setRotation(q);
   

   原点を設定するか rpy を設定するときは， 適用した全フィルタの最終結果を用いる必要があることに注意してください． ここでは原点は任意に最初の点に設定されます．

   変換をブロードキャスト:

   br.sendTransform(tf::StampedTransform(part_transform, ros::Time::now(), world_frame, "part"));
   

2. いつものようにコンパイルと実行をしてください． RViz の Display に TF を追加して， 箱の上部に位置づけられた TF "part" を表示します．

ポリゴン・セグメンテーションの作成

干渉検出にセンサ情報を使用する場合， これらのオブジェクトとの干渉を避けるために， 「既知の」オブジェクトをシーンから除外する必要があることがあります． MoveIt! にはロボット自身のジオメトリを 「自己干渉」フィルタリング機能としてマスクするためのメソッドが含まれています．

本項目では PCL のポリゴン・セグメンテーション・フィルタリングを使用して 同様のことを行う方法を提示します．

1. コードの変更

   この方法は3番目のサブタスクの平面セグメンテーションに似ていますが， 平面を分割する代わりにプリズムを分割して削除することができます．

   PCL ポリゴン・セグメンテーションに関するドキュメントは Point Cloud Library (PCL) 1.7 と Point Cloud Library (PCL) 1.9 にあります．

   本サブタスクの目標は， 既知のオブジェクト（例えば先に検出した箱）に対応するポイントを削除することです． この特殊なフィルタはポイントクラウド全体（元のセンサデータ）に適用しますが， 箱の位置・方向を特定するための処理手順を既に完了した後にだけ適用されます．

   perception_node.cpp に #include <tf_conversions/tf_eigen.h> を追加してから， 次のセクションを探してください．

   /* ========================================
    * Fill Code: POLYGONAL SEGMENTATION (OPTIONAL)
    * ========================================*/
   

   ポイントクラウド入力の設定:

   pcl::PointCloud<pcl::PointXYZ>::Ptr sensor_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>(cloud));
   pcl::PointCloud<pcl::PointXYZ>::Ptr prism_filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>::Ptr pick_surface_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
   

   フィルタ・インスタンスの宣言:

   pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism;
   

   抽出インデックスの設定:

   pcl::ExtractIndices<pcl::PointXYZ> extract_ind;
   

   入出力の設定:

   prism.setInputCloud(sensor_cloud_ptr);
   pcl::PointIndices::Ptr pt_inliers (new pcl::PointIndices());
   

   パラメータの設定:

   ドキュメントを見ると， ExtractPolygonalPrismData は入力としてポリゴン頂点を定義するポイントクラウドを使用します．

   // create prism surface
   double box_length=0.25;
   double box_width=0.25;
   pick_surface_cloud_ptr->width = 5;
   pick_surface_cloud_ptr->height = 1;
   pick_surface_cloud_ptr->points.resize(5);
   
   pick_surface_cloud_ptr->points[0].x = 0.5f*box_length;
   pick_surface_cloud_ptr->points[0].y = 0.5f*box_width;
   pick_surface_cloud_ptr->points[0].z = 0;
   
   pick_surface_cloud_ptr->points[1].x = -0.5f*box_length;
   pick_surface_cloud_ptr->points[1].y = 0.5f*box_width;
   pick_surface_cloud_ptr->points[1].z = 0;
   
   pick_surface_cloud_ptr->points[2].x = -0.5f*box_length;
   pick_surface_cloud_ptr->points[2].y = -0.5f*box_width;
   pick_surface_cloud_ptr->points[2].z = 0;
   
   pick_surface_cloud_ptr->points[3].x = 0.5f*box_length;
   pick_surface_cloud_ptr->points[3].y = -0.5f*box_width;
   pick_surface_cloud_ptr->points[3].z = 0;
   
   pick_surface_cloud_ptr->points[4].x = 0.5f*box_length;
   pick_surface_cloud_ptr->points[4].y = 0.5f*box_width;
   pick_surface_cloud_ptr->points[4].z = 0;
   
   Eigen::Affine3d eigen3d;
   tf::transformTFToEigen(part_transform,eigen3d);
   pcl::transformPointCloud(*pick_surface_cloud_ptr,*pick_surface_cloud_ptr,Eigen::Affine3f(eigen3d));
   
   prism.setInputPlanarHull( pick_surface_cloud_ptr);
   prism.setHeightLimits(-10,10);
   

   分割:

   prism.segment(*pt_inliers);
   

   セグメント化アルゴリズムを使用した後は， インデックス抽出を使用してセグメント化されたポイントを 含めるか除外するかを忘れないでください．

   入力の設定:

   extract_ind.setInputCloud(sensor_cloud_ptr);
   extract_ind.setIndices(pt_inliers);
   

   今回はインデックス抽出を逆にして， フィルタ内のポイントを削除し， フィルタ外のポイントを保持します．

   extract_ind.setNegative(true);
   

   フィルタ:

   extract_ind.filter(*prism_filtered_cloud);
   

2. pc2_cloud が入力されている pcl::toROSMsg 呼び出し部分を探してください． これは RViz ディスプレイで閲覧されるポイントクラウドです． 現在のクラウドを 新しいフィルタ結果の（ *prism_filtered_cloud ）に置き換えます．

3. いつものようにコンパイルと実行を行ってください．

   

   注釈

   ターゲットの箱がポイントクラウド表示から除かれていることに注目してください．

launch ファイルを書く

これはフィルターメソッドではありませんが， PCL または他の知覚メソッドを使用するときに， 異なるメソッドで使用されるパラメータの数が多くなるので便利です．

1. コードの変更・挿入

   最初のサブタスクに書いてあることを詳しく読んでいる人は， 1つの場所にパラメータを置くことが推奨されていたことに気付いているかと思います．

   perception_node.cpp の次のセクションを探してください．

   /*
    * SET UP PARAMETERS (COULD TO BE INPUT FROM LAUNCH FILE/TERMINAL)
    */
   

   下のパラメータの例が示すように 理想的には特定の型（ std::string frame; ）のパラメータを宣言し， そのパラメータの値を代入します（ frame="some_name"; ）． 下記は設定可能なパラメータの例です．

   world_frame="kinect_link";
   camera_frame="kinect_link";
   cloud_topic="kinect/depth_registered/points";
   voxel_leaf_size=0.002f;
   x_filter_min=-2.5;
   x_filter_max=2.5;
   y_filter_min=-2.5;
   y_filter_max=2.5;
   z_filter_min=-2.5;
   z_filter_max=1.0;
   plane_max_iter=50;
   plane_dist_thresh=0.05;
   cluster_tol=0.01;
   cluster_min_size=100;
   cluster_max_size=50000;
   

   本サブタスクの手順により， パラメータを launch ファイルまたは yaml ファイルから入力できるものに変えることができます． チュートリアル Using Parameters in roscpp で説明されているように，"getParam" メソッドを使用することができます． しかし，より良い選択は param メソッドを使うことです． param メソッドはパラメータがパラメータサーバ上にない場合にはデフォルト値を返します． ROS パラメータサーバもしくは launch ファイルから params を取得して， 前回のハードコードされた値を置き換えてください． （ただし変数宣言は残してください！）

   cloud_topic = priv_nh_.param<std::string>("cloud_topic", "kinect/depth_registered/points");
   world_frame = priv_nh_.param<std::string>("world_frame", "kinect_link");
   camera_frame = priv_nh_.param<std::string>("camera_frame", "kinect_link");
   voxel_leaf_size = param<float>("voxel_leaf_size", 0.002);
   x_filter_min = priv_nh_.param<float>("x_filter_min", -2.5);
   x_filter_max = priv_nh_.param<float>("x_filter_max",  2.5);
   y_filter_min = priv_nh_.param<float>("y_filter_min", -2.5);
   y_filter_max = priv_nh_.param<float>("y_filter_max",  2.5);
   z_filter_min = priv_nh_.param<float>("z_filter_min", -2.5);
   z_filter_max = priv_nh_.param<float>("z_filter_max",  2.5);
   plane_max_iter = priv_nh_.param<int>("plane_max_iterations", 50);
   plane_dist_thresh = priv_nh_.param<float>("plane_distance_threshold", 0.05);
   cluster_tol = priv_nh_.param<float>("cluster_tolerance", 0.01);
   cluster_min_size = priv_nh_.param<int>("cluster_min_size", 100);
   cluster_max_size = priv_nh_.param<int>("cluster_max_size", 50000);
   

2. launch ファイルの記述

   gedit もしくは他のテキストエディタを使用して， 新しいファイル（ "lesson_perception/launch/processing_node.launch" ）を作成して， 下記内容を書き入れてください．

   <launch>
     <node name="processing_node" pkg="lesson_perception" type="perception_node" output="screen">
       <rosparam>
         cloud_topic: "kinect/depth_registered/points"
         world_frame: "world_frame"
         camera_frame: "kinect_link"
         voxel_leaf_size: 0.001 <!-- mm -->
         x_filter_min: -2.5 <!-- m -->
         x_filter_max: 2.5 <!-- m -->
         y_filter_min: -2.5 <!-- m -->
         y_filter_max: 2.5 <!-- m -->
         z_filter_min: -2.5 <!-- m -->
         z_filter_max: 2.5 <!-- m -->
         plane_max_iterations: 100
         plane_distance_threshold: 0.03
         cluster_tolerance: 0.01
         cluster_min_size: 250
         cluster_max_size: 500000
       </rosparam>
     </node>
   </launch>
   

3. コンパイル

   今回は rosrun を使用する代わりに， 新しく作成した launch ファイルを実行して処理ノードを開始します．

   結果は以前の実行と同様に見えるはずです． しかし，これらの設定パラメータをずっと簡単に編集できるようになりました！ 再コンパイルは不要で，launch ファイルの値を編集してノードを再起動するだけです． 実際のアプリケーションではこのアプローチをさらに進めて ノードで dynamic_reconfigure サポートを実装することができます． これにより RViz でリアルタイムに パラメータ変更の結果を確認することができるようになります．

   結果が良いようでしたら，各端末で CTRL-C を行い終了します．

   本節の演習はすべて終わりました！

STOMP 入門

モチベーション

• STOMP ( Stochastic Trajectory Optimization for Motion Planning / 運動計画のための確率的軌道最適化 ) を利用した MoveIt! での動作計画方法の学習

情報とリソース

• STOMP for MoveIt!
• Plugins for MoveIt!

目標

• moveit_config パッケージにファイルを追加・変更して MoveIt! に STOMP を統合
• RViz Motion Planning Plugin で STOMP を用いた動作計画の生成

セットアップ

• ワークスペースを作成します．

  mkdir --parent ~/catkin_ws/src
  cd ~/catkin_ws
  catkin init
  catkin build
  source devel/setup.bash
  

• 既存の演習内容をコピーします．

  cd ~/catkin_ws/src
  cp -r ~/industrial_training/exercises/4.1 .
  

• ワークスペースに industrial_moveit リポジトリをコピーします．

  cd ~/catkin_ws/src
  git clone https://github.com/ros-industrial/industrial_moveit.git
  cd ~/catkin_ws/src/industrial_moveit
  git checkout kinetic-devel
  

• 足りない依存関係にあるパッケージをインストールします．

  cd ~/catkin_ws/src/4.1
  rosinstall . .rosinstall
  catkin build
  

• moveit_config パッケージを MoveIt! Setup Assistant を用いて作成します．

STOMP の追加

1. "stomp_planning_pipeline.launch.xml" ファイルを moveit_config パッケージの launch ディレクトリに作成します． このファイルには次の内容を書き込んでください．

   <launch>
   
     <!-- Stomp Plugin for MoveIt! -->
     <arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />
   
     <!-- The request adapters (plugins) ORDER MATTERS -->
     <arg name="planning_adapters" value="default_planner_request_adapters/FixWorkspaceBounds
                                          default_planner_request_adapters/FixStartStateBounds
                                          default_planner_request_adapters/FixStartStateCollision
                                          default_planner_request_adapters/FixStartStatePathConstraints" />
   
     <arg name="start_state_max_bounds_error" value="0.1" />
   
     <param name="planning_plugin" value="$(arg planning_plugin)" />
     <param name="request_adapters" value="$(arg planning_adapters)" />
     <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
     <rosparam command="load" file="$(find myworkcell_moveit_config)/config/stomp_planning.yaml"/>
   
   </launch>
   

   !!! launch ファイル内の設定ファイル stomp_planning.yaml の記述に注目してください． このファイルは moveit_config パッケージ内に置く必要があります．

2. 設定ファイル "stomp_planning.yaml" の作成

   このファイルには　STOMP で必要となるパラメータを記述します． パラメータは SRDF ファイルで定義されている各 "Planning Group" に固有なものとなります． したがって，3つのプランニング・グループ

   • manipulator
   • manipulator_tool
   • manipulator_rail

   が存在する場合， 設定ファイルでは各プランニング・グループごとに特定のパラメータセットを定義します．

   stomp/manipulator_rail:
     group_name: manipulator_rail
     optimization:
       num_timesteps: 60
       num_iterations: 40
       num_iterations_after_valid: 0    
       num_rollouts: 30
       max_rollouts: 30
       initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST
       control_cost_weight: 0.0
     task:
       noise_generator:
         - class: stomp_moveit/NormalDistributionSampling
           stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4, 0.4]
       cost_functions:
         - class: stomp_moveit/CollisionCheck
           collision_penalty: 1.0
           cost_weight: 1.0
           kernel_window_percentage: 0.2
           longest_valid_joint_move: 0.05
       noisy_filters:
         - class: stomp_moveit/JointLimits
           lock_start: True
           lock_goal: True
         - class: stomp_moveit/MultiTrajectoryVisualization
           line_width: 0.02
           rgb: [255, 255, 0]
           marker_array_topic: stomp_trajectories
           marker_namespace: noisy
       update_filters:
         - class: stomp_moveit/PolynomialSmoother
           poly_order: 6
         - class: stomp_moveit/TrajectoryVisualization
           line_width: 0.05
           rgb: [0, 191, 255]
           error_rgb: [255, 0, 0]
           publish_intermediate: True
           marker_topic: stomp_trajectory
           marker_namespace: optimized
   stomp/manipulator:
     group_name: manipulator
     optimization:
       num_timesteps: 40
       num_iterations: 40
       num_iterations_after_valid: 0    
       num_rollouts: 10
       max_rollouts: 10
       initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST
       control_cost_weight: 0.0
     task:
       noise_generator:
         - class: stomp_moveit/NormalDistributionSampling
           stddev: [0.05, 0.4, 1.2, 0.4, 0.4, 0.1]
       cost_functions:
         - class: stomp_moveit/CollisionCheck
           kernel_window_percentage: 0.2
           collision_penalty: 1.0
           cost_weight: 1.0
           longest_valid_joint_move: 0.05
       noisy_filters:
         - class: stomp_moveit/JointLimits
           lock_start: True
           lock_goal: True
         - class: stomp_moveit/MultiTrajectoryVisualization
           line_width: 0.04
           rgb: [255, 255, 0]
           marker_array_topic: stomp_trajectories
           marker_namespace: noisy
       update_filters:
         - class: stomp_moveit/PolynomialSmoother
           poly_order: 5
         - class: stomp_moveit/TrajectoryVisualization
           line_width: 0.02
           rgb: [0, 191, 255]
           error_rgb: [255, 0, 0]
           publish_intermediate: True
           marker_topic: stomp_trajectory
           marker_namespace: optimized      
   

   !!! このファイルを moveit_config パッケージの config ディレクトリに保存します．

3. move_group.launch ファイルを修正します．

   launch ディレクトリ内の move_group.launch ファイルを開いて， 次のように pipeline パラメータの値を stomp に変更します．

       .
       .
       .
   <!-- move_group settings -->
   <arg name="allow_trajectory_execution" default="true"/>
   <arg name="fake_execution" default="false"/>
   <arg name="max_safe_path_cost" default="1"/>
   <arg name="jiggle_fraction" default="0.05" />
   <arg name="publish_monitored_planning_scene" default="true"/>
   
   <!-- Planning Functionality -->
   <include ns="move_group" file="$(find myworkcell_moveit_config)/launch/planning_pipeline.launch.xml">
     <arg name="pipeline" value="stomp" />
   </include>
   
       .
       .
       .
   

STOMP とともに MoveIt! を実行

1. 2つ目のターミナルで demo.lauch ファイルを実行します．

   roslaunch myworkcell_moveit_config demo.launch
   

2. RViz にてロボットの "Start" と "Goal" の姿勢を選択して動作計画をします．

• "Motion Planning" パネル内の "Planning" タブに移動します．
• "Select Start State" ドロップダウンをクリックして， "allZeros" を選択してから "Update" をクリックします．
• "Select Goal State" ドロップダウンをクリックして， "home" を選択してから "Update" をクリックします．
• "Plan" ボタンをクリックして， 目標位置に到達するために腕が障害物を越えていくのを観察してください． 青い線はツールの軌道を示します．

STOMP の探索

1. RViz で他の "Start" と "Goal" の位置を選択して， "Plan" を行いロボットの動きを見てください．
2. "Displays" パネルにある "Marker Array" チェックボックをクリックして Noisy Trajectories を表示してください． 再び "Plan" ボタンをクリックすると， 黄色い線の "Noisy Trajectory Markers" が表示されるはずです．

STOMP は現在の軌道にノイズを適用した結果として 多数のノイジーな軌道を生成することによって， 作業空間を探索します． 適用されるノイズの度合いは "stomp_config.yaml" ファイルの "stddev" パラメータを調整することで変更できます． より大きい "stddev" 値は関節のより大きな動きに相当します．

STOMP の設定

stomp_config.yaml のパラメータを変更し， これらの変更が動作計画にどのような影響を与えるかを確認します．

1. 先程 demo.launch ファイルを実行したターミナルで Ctrl-C を入力してノードを停止します．
2. エディタで stomp_config.yaml ファイルを開きます．
3. "manipulator_rail" グループ内の "stddev" パラメータに割り当てられた値に注目してください． 配列内の各値が関節に適用されるノイズの振幅です． 例えば配列の左端の値は最初の関節 "rail_to_base" のノイズを設定するための値になります． レールは x 方向に沿って移動します． "rail_to_base" は直道関節ですので単位はメートル [m] です． 回転関節の場合は，単位はラジアン [rad] です．
4. "stddev" の値を変更して（好ましくは一度に1つの変更）， ファイルを保存してからターミナルで "demo.launch" ファイルを実行してください．
5. RViz ウィンドウに戻って， 任意の "Start" と "Goal" の位置を選択して， 変更が動作計画のパフォーマンスに与える影響を確認してください．

コードは industrial_training リポジトリの gh_pages フォルダにあります． kinetic ブランチを使用してください．

Python のためのシンプルな PCL インタフェースの構築

本演習では知覚パイプラインを構築するための適切なコードを記入します． 最終目標は ROS と Python 間の機能性を見るために， ポイントクラウドのフィルタリング処理を作成します．

新規ワークスペースの準備

本演習はこれまでの Plan-N-Scan 演習とは独立した内容ですので， 新しく catkin ワークスペースを作成します．

1. 既存の catkin ワークスペース環境を自動的にセットアップする設定を止めます．

   1. gedit ~/.bashrc
   2. source ~/catkin_ws/devel/setup.bash している 最終行を # でコメントアウトしてください．

   source /opt/ros/kinetic/setup.bash
   # source ~/catkin_ws/devel/setup.bash
   

2. ワークスペース・レイアウト／ファイルのテンプレートをコピーしてください．

   cp -r ~/industrial_training/exercises/python-pcl_ws ~
   cd ~/python-pcl_ws/
   

3. ワークスペースの初期化とビルドを行います．

   catkin init
   catkin build
   

4. ワークスペースのセットアップを行います．

   source ~/python-pcl_ws/devel/setup.bash
   

5. ポイントクラウドファイルをダンロードしてホームディレクトリ（~）に置いてください．

6. 新しいワークスペースを QTCreator IDE にインポートしてください．

   • In QTCreator: File -> New Project -> Import -> Import ROS Workspace -> ~/python-pcl_ws

既存コードの確認

ROS ノードの基盤部分の大半は既に完成していますので， 本演習の焦点は知覚アルゴリズムとそのパイプラインです． CMakelists.txt と package.xml と完成したソースファイルも提供されています． ソースコードをそのままビルドすることはできますがエラーが発生します．

ここでは提供されているソースコードを見ていきます． py_perception_node.cpp ファイルを参照してください．

本チュートリアルはトレーニング 演習 5.1 知覚パイプラインの構築 の内容を Python 向けに改良したもので， 演習 5.1 の C++ コードは既にセットアップ済みとして進めます． 何か分からない内容があれば「演習 5.1 知覚パイプラインの構築」に戻って確認してください．

それでは py_perception_node.cpp ファイルを開いて各フィルタ機能について見ていきます．

Python パッケージの作成

いくつかのフィルタを C++ 関数に転換してあるので， Python ノードから呼び出す準備は整っています．

PyCharm の Community Edition を インストールしていない場合はインストールしてください． この IDE には，編集するために必要なパーサがあります． これがなければ Qt で構文エラーを確認することはできません．

1. ターミナルで src フォルダに移動してください． pyrhon-pcl_ws 内に新しいパッケージを作成します．

   cd ~/python-pcl_ws/src/
   catkin_create_pkg filter_call rospy roscpp perception_msgs
   

   注釈

   訳注 - 後述されますが今回 perception_msgs は使用せず， ビルドや実行時にエラーが出ることがあります． その場合は CMakeList.text と package.xml 内の perception_msgs に関する依存関係の記述部分を コメントアウトして無効化してください．

2. パッケージが新しく作成できたことを確認します．

   ls
   

本演習ではカスタム・メッセージを作成しないため， 'perception_msgs' は使用しません． それは更に学習が進んだ時点で学んでください． カスタム・メッセージを実装する方法を含め，より深い説明が必要な場合は ここに良い MIT Resource がありあす．

1. CMakeLists.txt を開いてください． Pycharm や Qt で（もしくは nano，emacs，vim，subilme でも）開くことができます．

   下記内容の行（23行目付近）のコメントアウトを外して，ファイルを保存してください．

   catkin_python_setup()
   

setup.py の作成

setup.py ファイルは Python モジュールを ワークスペース全体とその後のパッケージで利用できるようにします． デフォルトでは catkin_create_pkg コマンドでは作成されません．

1. ターミナルで次のように入力してください．

   gedit filter_call/setup.py
   

2. 下記内容を setup.py にコピー＆ペーストしてください．

   ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
   from distutils.core import setup
   from catkin_pkg.python_setup import generate_distutils_setup
   # fetch values from package.xml
   setup_args = generate_distutils_setup(
   packages=[''],
   package_dir={'': 'include'},
   )
   setup(**setup_args)
   

   packages = [ . . . ], の内容を変更して， include フォルダ内のフォルダ名の文字列リストを追加します． 慣習的にはパッケージと同じ名前か filter_call です． これは filter_call/include/filter_call を ワークスペース全体で利用可能な Python モジュールとして設定します．

3. ファイルを保存して閉じてください．

   このフォルダに他の Python スクリプトがアクセスできるようにするには， __init__.py ファイルが存在しなければなりません。

4. __init__.py ファイルの作成

   ターミナルで次のように入力してファイルを作成します．

   touch filter_call/include/filter_call/__init__.py
   

ポイントクラウドのパブリッシュ

ポイントクラウドをフィルタリングする ROS C++ ノードは既に作成されていて， 各フィルタ操作のためのサービスリクエストを Python ノードで実行した時に 新しいポイントクラウドが作成されます．

まず Python をサポートする方法で公開するように C++ コードを修正します． C++ のコードが完成すれば， 必要があるのは Python スクリプトを書いて結果を RViz で見ることだけです．

Voxel フィルタの実装

1. py_perception_node.cpp 内の filterCallBack という名前のブーリアン関数（ main 関数の真上 ） のコメントアウトを外して実行できるようにします． これは後続のフィルタリング操作を実行するために Python クライアントによって利用されるサービスになります．

   bool filterCallback(lesson_perception::FilterCloud::Request& request,
                       lesson_perception::FilterCloud::Response& response)
   {
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
   
     if (request.pcdfilename.empty())
     {
       pcl::fromROSMsg(request.input_cloud, *cloud);
       ROS_INFO_STREAM("cloud size: " << cloud->size());
     }
     else
     {
       pcl::io::loadPCDFile(request.pcdfilename, *cloud);
     }
   
     if (cloud->empty())
     {
       ROS_ERROR("input cloud empty");
       response.success = false;
       return false;
     }
   
     switch (request.operation)
     {
   
       case lesson_perception::FilterCloud::Request::VOXELGRID :
       {
         filtered_cloud = voxelGrid(cloud, 0.01);
         break;
       }
       default :
       {
         ROS_ERROR("No valid request found");
         return false;
       }
   
      }
   
   /*
    * SETUP RESPONSE
    */
     pcl::toROSMsg(*filtered_cloud, response.output_cloud);
     response.output_cloud.header=request.input_cloud.header;
     response.output_cloud.header.frame_id="kinect_link";
     response.success = true;
     return true;
   }
   

2. main 関数内の240行目付近の下記内容の行の コメントアウトを外してから保存し，ビルドします．

   priv_nh_.param<double>("leaf_size", leaf_size_, 0.0f);
   

3. これでフィルタリングのためのフレームワークが完成しました． ターミナルを開いて filter_call ディレクトリにいることを確認して， そこで scripts フォルダを作成してください．

   mkdir scripts
   

4. Pycharm が開いている場合は保存して閉じます． 端末から Pycharm を開いて C++ ノードが正しく見て取れるかを確認します．． Pycharm を開くためには Pycharm のインストールディレクトリに移動します．

   cd ~/pycharm-community-2018.1.3/bin
   ./pycharm.sh
   

   Pycharm を開いたら フォルダ scripts を探して右クリックし，新しい Python ファイルを作成します． それに filter_call.py というファイル名をつけます．

5. 必要なライブラリをインポートするには， filter_call.py に次のコードをコピー＆ペーストしてください．

   #!/usr/bin/env python
   
   import rospy
   import lesson_perception.srv
   from sensor_msgs.msg import PointCloud2
   

6. このファイルが実行されたときに Python ノードを実行するための if 文を作成します． 次のように初期化します．

   if __name__ == '__main__':
       try:
   
       except Exception as e:
           print("Service call failed: %s" % str(e))
   

7. 次のように rospy.spin() を try ブロック内に設置します．

   if __name__ == '__main__':
       try:
           rospy.spin()
       except Exception as e:
           print("Service call failed: %s" % str(e))
   

8. try ブロック内に次の内容をコピー＆ペーストしてください．

   # =======================
   # VOXEL GRID FILTER
   # =======================
   
   srvp = rospy.ServiceProxy('filter_cloud', lesson_perception.srv.FilterCloud)
   req = lesson_perception.srv.FilterCloudRequest()
   req.pcdfilename = rospy.get_param('~pcdfilename', '')
   req.operation = lesson_perception.srv.FilterCloudRequest.VOXELGRID
   # FROM THE SERVICE, ASSIGN POINTS
   req.input_cloud = PointCloud2()
   
   # ERROR HANDLING
   if req.pcdfilename == '':
       raise Exception('No file parameter found')
   
   # PACKAGE THE FILTERED POINTCLOUD2 TO BE PUBLISHED
   res_voxel = srvp(req)
   print('response received')
   if not res_voxel.success:
       raise Exception('Unsuccessful voxel grid filter operation')
   
   # PUBLISH VOXEL FILTERED POINTCLOUD2
   pub = rospy.Publisher('/perception_voxelGrid', PointCloud2, queue_size=1, latch=True)
   pub.publish(res_voxel.output_cloud)
   print("published: voxel grid filter response")
   

9. Python ノードを初期化して C++ ノードのサービスを待つために， try ブロックの上に（ if ステートメント内に ）次の行を貼り付けます．

   rospy.init_node('filter_cloud', anonymous=True)
   rospy.wait_for_service('filter_cloud')
   

10. Python ファイルを実行可能なものとするために， ターミナルから下記コマンドを入力してファイルに実行権限を付与します．

    chmod +x filter_call/scripts/filter_call.py
    

結果を見る

1. ターミナルから次を実行してください．

   roscore
   

2. 新しいターミナルで ROS 環境設定を行った後， C++ のフィルタサービス・ノードを実行します．

   rosrun lesson_perception py_perception_node
   

3. 新しいターミナルで ROS 環境設定を行った後， Python のサービスコール・ノードを実行します．

   ファイルパスが違うことがありますので注意してください．

   rosrun filter_call filter_call.py _pcdfilename:="/home/ros-industrial/catkin_ws/table.pcd"
   

4. 新しいターミナルで ROS 環境設定を行った後， RViz を起動します．

   rosrun rviz rviz
   

5. RViz に新しく PointCloud2 を追加してください．

6. RViz の Global Options の Fixed Frame を kinect_link に変更してください． そして PointCloud2 のトピックに '/perception_voxelGrid' を選択してください．

   注釈

   PointCloud2 のチェックボックスを切手から再び入れる必要があることがあります．

パススルーフィルタの実装

1. lesson_perception パッケージの py_perception_node.cpp の main 関数内の 次の2つの行のコメントを外し， また28行目と29行目付近のそれらの変数の宣言部分のコメントも解除してください．

   priv_nh_.param<double>("passThrough_max", passThrough_max_, 1.0f);
   priv_nh_.param<double>("passThrough_min", passThrough_min_, -1.0f);
   

2. switch の内容を次のように変更してください．

   switch (request.operation)
   {
   
     case lesson_perception::FilterCloud::Request::VOXELGRID :
     {
       filtered_cloud = voxelGrid(cloud, 0.01);
       break;
     }
     case lesson_perception::FilterCloud::Request::PASSTHROUGH :
     {
       filtered_cloud = passThrough(cloud);
       break;
     }
     default :
     {
       ROS_ERROR("No valid request found");
       return false;
     }
   
   }
   

3. 保存してビルドしてください．

   Python コードの編集

4. Python ノードのファイルを開き， 次のコードをボクセルグリッドの後，rospy.spin() の前に貼り付けます． インデントを崩さないように注意してください．

   # =======================
   # PASSTHROUGH FILTER
   # =======================
   
   srvp = rospy.ServiceProxy('filter_cloud', lesson_perception.srv.FilterCloud)
   req = lesson_perception.srv.FilterCloudRequest()
   req.pcdfilename = ''
   req.operation = lesson_perception.srv.FilterCloudRequest.PASSTHROUGH
   # FROM THE SERVICE, ASSIGN POINTS
   req.input_cloud = res_voxel.output_cloud
   
   # PACKAGE THE FILTERED POINTCLOUD2 TO BE PUBLISHED
   res_pass = srvp(req)
   print('response received')
   if not res_voxel.success:
       raise Exception('Unsuccessful pass through filter operation')
   
   # PUBLISH PASSTHROUGH FILTERED POINTCLOUD2
   pub = rospy.Publisher('/perception_passThrough', PointCloud2, queue_size=1, latch=True)
   pub.publish(res_pass.output_cloud)
   print("published: pass through filter response")
   

5. 保存後に端末から実行し Voxel フィルタで説明した手順を繰り返します．

   RViz で ``/kinect/depth_registered/points``（元のカメラデータ）を基にした PointCloud2 と ``perception_passThrough``（ここで行った処理データ）の PointCloud2 を比べてください．

   ここでのフィルタ処理でポイントクラウドの一部が「切り取り」されているはずです．

   パススルーフィルタの結果が良いようでしたら Ctrl+C でノードを停止してください． 他のターミナルやノードを停止する必要はありません．

平面のセグメンテーション

この方法は対象物が平坦な表面上にあるあらゆるアプリケーションにとって最も有用な方法の1つです． テーブル上のオブジェクトを分離するには， ポイント群を平面にフィットさせ， テーブルを構成するポイント群を見つけ， それらのポイント群を除外して， テーブル上のオブジェクトに対応するポイント群だけを残すようにします． これは私たちが使用する最も複雑な PCL メソッドで， 実際には RANSAC セグメンテーションモデルと 抽出インデックスツールの2つの組み合わせとなります． 詳細な例は PCL Plane Model Segmentation Tutorial にあります． もしくは下記コードをコピーして使用してください．

1. py_perception_node.cpp の main 関数内の 下記2行とともに，それらの変数の宣言部分もコメントアウトを外してください．

   priv_nh_.param<double>("maxIterations", maxIterations_, 200.0f);
   priv_nh_.param<double>("distThreshold", distThreshold_, 0.01f);
   

2. filterCallback 内の switch を次のように変更してください．

   switch (request.operation)
   {
   
     case lesson_perception::FilterCloud::Request::VOXELGRID :
     {
       filtered_cloud = voxelGrid(cloud, 0.01);
       break;
     }
     case lesson_perception::FilterCloud::Request::PASSTHROUGH :
     {
       filtered_cloud = passThrough(cloud);
       break;
     }
     case lesson_perception::FilterCloud::Request::PLANESEGMENTATION :
     {
       filtered_cloud = planeSegmentation(cloud);
       break;
     }
     default :
     {
       ROS_ERROR("No valid request found");
       return false;
     }
   
   }
   

3. 保存してビルドしてください．

   Python コードの編集

4. 次のコードを filter_call.py 内のパススルーフィルタ部分の後にコピー＆ペーストしてください． インデントが崩れないように注意してください．

   # =======================
   # PLANE SEGMENTATION
   # =======================
   
   srvp = rospy.ServiceProxy('filter_cloud', lesson_perception.srv.FilterCloud)
   req = lesson_perception.srv.FilterCloudRequest()
   req.pcdfilename = ''
   req.operation = lesson_perception.srv.FilterCloudRequest.PLANESEGMENTATION
   # FROM THE SERVICE, ASSIGN POINTS
   req.input_cloud = res_pass.output_cloud
   
   # PACKAGE THE FILTERED POINTCLOUD2 TO BE PUBLISHED
   res_seg = srvp(req)
   print('response received')
   if not res_voxel.success:
       raise Exception('Unsuccessful plane segmentation operation')
   
   # PUBLISH PLANESEGMENTATION FILTERED POINTCLOUD2
   pub = rospy.Publisher('/perception_planeSegmentation', PointCloud2, queue_size=1, latch=True)
   pub.publish(res_seg.output_cloud)
   print("published: plane segmentation filter response")
   

5. 保存後に端末から実行し Voxel フィルタで説明した手順を繰り返します．

   RViz で ``/kinect/depth_registered/points``（元のカメラデータ）を基にした PointCloud2 と ``perception_planeSegmentation``（ここで行った処理データ）の PointCloud2 を比べてください． 処理した結果ではテーブル平面の上にある点だけが残っているはずです．

   1. 結果の表示が終了したら，"setMaxIterations" および "setDistanceThreshold" の値を変更して， 平面適合点／不適合点としてデータをどれくらい厳密に制御できるかを試して， その結果を表示してみてください． MaxIterations = 100 と DistanceThreshold = 0.010 の値を試してみてください．
   2. 平面分割の結果を確認できたら，Ctrl+C キーを使用してノードを停止してください． 他のターミナルを閉じたり他のノードを停止したりする必要はありません．

ユークリッドクラスタの抽出

この方法は複数のオブジェクトがあるすべてのアプリケーションにおいて有用です． またこれは複雑な PCL メソッドでもあります． 詳細な例は PCL Euclidean Cluster Extration Tutorial にあります．

1. py_perception_node.cpp の main 関数内の 下記行とともに，それらの変数の宣言部分もコメントアウトを外してください．

   priv_nh_.param<double>("clustTol", clustTol_, 0.01f);
   priv_nh_.param<double>("clustMax", clustMax_, 10000.0);
   priv_nh_.param<double>("clustMin", clustMin_, 300.0f);
   

2. filterCallback 内の switch を次のように変更してください．

   switch (request.operation)
   {
   
     case lesson_perception::FilterCloud::Request::VOXELGRID :
     {
       filtered_cloud = voxelGrid(cloud, 0.01);
       break;
     }
     case lesson_perception::FilterCloud::Request::PASSTHROUGH :
     {
       filtered_cloud = passThrough(cloud);
       break;
     }
     case lesson_perception::FilterCloud::Request::PLANESEGMENTATION :
     {
       filtered_cloud = planeSegmentation(cloud);
       break;
     }
     case lesson_perception::FilterCloud::Request::CLUSTEREXTRACTION :
     {
       std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> temp =clusterExtraction(cloud);
       if (temp.size()>0)
       {
         filtered_cloud = temp[0];
       }
       break;
     }
     default :
     {
       ROS_ERROR("No valid request found");
       return false;
     }
   
   }
   

3. 保存してビルドしてください．

   Python コードの編集

4. 次のコードを filter_call.py 内の平面セグメンテーション部分の後にコピー＆ペーストしてください． インデントが崩れないように注意してください．

   # =======================
   # CLUSTER EXTRACTION
   # =======================
   
   srvp = rospy.ServiceProxy('filter_cloud', lesson_perception.srv.FilterCloud)
   req = lesson_perception.srv.FilterCloudRequest()
   req.pcdfilename = ''
   req.operation = lesson_perception.srv.FilterCloudRequest.CLUSTEREXTRACTION
   # FROM THE SERVICE, ASSIGN POINTS
   req.input_cloud = res_seg.output_cloud
   
   # PACKAGE THE FILTERED POINTCLOUD2 TO BE PUBLISHED
   res_cluster = srvp(req)
   print('response received')
   if not res_voxel.success:
       raise Exception('Unsuccessful cluster extraction operation')
   
   # PUBLISH CLUSTEREXTRACTION FILTERED POINTCLOUD2
   pub = rospy.Publisher('/perception_clusterExtraction', PointCloud2, queue_size=1, latch=True)
   pub.publish(res_cluster.output_cloud)
   print("published: cluster extraction filter response")
   

5. 保存後に端末から実行し Voxel フィルタで説明した手順を繰り返します．

   1. クラスタ抽出の結果が良いようでしたら，Ctrl+C キーを使用してノードを停止してください． 本チュートリアルは以上ですので，終えるようでしたら他のターミナルのノードも停止してください．

将来的な学習

演習 5.1 を呼び出し可能な関数に変換し， より良くフィルタリング処理できるようにすることをお勧めします．

本演習では単純化のために 各フィルタリングインスタンスごとに Python コードを繰り返しました． 大量のコードを繰り返す代わりに パブリッシングを処理するためのループを作成することをお勧めします． また，デフォルトのものを使うだけではなく， パラメータ操作の全機能を活用することもでき， それらを Python から設定することもできます． これらの関数呼び出しを作成する場合， ここで説明されたもの以外にもいくつかのフィルタリング操作があります．

OpenCV 画像処理（ Pyhton ）

本演習では簡単な 2D 画像処理アプリケーションを通じて OpenCV と Python の両方に親しんでいきます．

モチベーション

OpenCV は成熟し安定した 2D 画像処理ライブラリで， 多種多様なアプリケーションで利用されています． ROS の多くは 3D センサーとポイントクラウドデータを利用していますが， 従来の 2D カメラや画像処理を使用するアプリケーションはまだまだ多くあります．

本演習では Python を使用して画像処理パイプラインを構築します． Python は迅速なプロトタイプ作成が容易で OpenCV ライブラリへの接合手段が既にあるため このアプリケーションに適しています．

情報とリソース

• OpenCV Website
• OpenCV API
• OpenCV Python Tutorials
• Converting between ROS images and OpenCV images (Python)
• Writing a Publisher and Subscriber (Python)
• sensor_msgs/Image
• シンプルな Publisher と Subscriber を書く (Python)

演習問題

本演習では OpenCV 画像処理ライブラリを用いて ポンプハウジングの角度姿勢を特定する新しいノードを作成します． ポンプの向きは，ジオメトリフィーチャを抽出して比較する 一連の処理ステップを使用して計算されます．

1. 画像のリサイズ（処理の高速化のため）
2. 画像の閾値処理（白黒への変換）
3. ポンプのアウターハウジングの位置の特定（円の抽出）
4. ピストン・スリーブ位置の特定（連結領域 blob の検出）
5. バウンディング・ボックスを使用して主軸を推定
6. ピストン・スリーブの位置を使用して方向を特定
7. 基準（水平）軸に対する軸の向きを計算

実装

パッケージの作成

この演習では任意の catkin ワークスペースに配置できる単一のパッケージを使用します． 以下の例では，以前の演習の ~/catkin_ws ワークスペースを使用します．

1. これから作成する新しい Python ノードを格納する新しいパッケージ detect_pump を作成します．

   cd ~/catkin_ws/src
   catkin create pkg detect_pump --catkin-deps rospy cv_bridge
   

   • 全ての ROS パッケージは rospy に依存します．
   • cv_bridge を使って ROS の標準画像メッセージと OpenCV の画像オブジェクトを変換します．
   • また cv_bridge は依存関係にある OpenCV モジュールを自動的に引っ張ってきます．

2. このパッケージの Python モジュールを作成します．

   cd detect_pump
   mkdir nodes
   

   • このようなシンプルなパッケージのために Python Style Guide はパッケージ構造を完結にすることを薦めています．
   • より複雑なパッケージ（例えばエクスポート可能なモジュール，msg/srv 定義など）は， __init__.py と setup.py を使ってより複雑なパッケージ構造にしてください．
     • 参考
       • Installing Python Scripts
       • Handling setup.py

画像パブリッシャの作成

まず最初のノードは ファイルから画像を読み込んで ROS の image トピック上の Image メッセージとしてパブリッシュします．

• Note: ROS にはこの機能を実行する既存の image_publisher パッケージ/ノードがありますが， ここではそれをコピーして Python の ROS Publishers について学んでゆきます．

1. 画像パブリッシャ・ノード（ nodes/image_pub.py ）用の 新しい Python スクリプトを作成します． ROS Python ノードのスケルトン・テンプレートに次のコードを書き入れてください．

   #!/usr/bin/env python
   import rospy
   
   def start_node():
       rospy.init_node('image_pub')
       rospy.loginfo('image_pub node started')
   
   if __name__ == '__main__':
       try:
           start_node()
       except rospy.ROSInterruptException:
           pass
   

2. 新しいスクリプトファイルに実行権限を付与します．

   chmod u+x nodes/image_pub.py
   

3. 画像パブリッシャをテスト実行します．

   roscore
   rosrun detect_pump image_pump.py
   

   • "node started" と表示されるはずです．

4. コマンドラインから提供されたファイル名を使用して，公開するイメージファイルを読み込みます．

   1. sys と cv2 (OpenCV) モジュールをインポートします．

      import sys
      import cv2
      

   2. コマンドライン引数を start_node 関数に渡します．

      def start_node(filename):
      ...
      start_node( rospy.myargv(argv=sys.argv)[1] )
      

      • ROS 固有のコマンドライン引数を取り除くために rospy.myargv() を使用することに注意してください．

   3. start_node 関数内で OpenCV を呼び出します．

      • imread 関数が画像を読み込みます．
      • imshow を使って表示します．

      img = cv2.imread(filename)
      cv2.imshow("image", img)
      cv2.waitKey(2000)
      

   4. 画像ファイルを指定してノードを実行してください．

      rosrun detect_pump image_pub.py ~/industrial_training/exercises/5.4/pump.jpg
      

      • ディスプレイに画像が表示されるはずです．
      • imshow/waitKey をコメントアウトすると，それらが表示されなくなります．
      • Python ファイルを編集した後にコンパイルは必要ありませんので catkin build を行う必要はありません．

5. 画像を OpenCV オブジェクトから ROS の Image メッセージに変換します．

   1. CvBridge と Image（ROS メッセージ）モジュールをインポートします．

      from cv_bridge import CvBridge
      from sensor_msgs.msg import Image
      

   2. CvBridge cv2_to_imgmsg メソッドへの呼び出しを追加します．

      bridge = CvBridge()
      imgMsg = bridge.cv2_to_imgmsg(img, "bgr8")
      

6. Image メッセージを継続的に image トピックに公開するために ROS パブリッシャを作成します． メッセージを発行するために 1 Hz のスロットルを持つループを使用します．

   pub = rospy.Publisher('image', Image, queue_size=10)
   while not rospy.is_shutdown():
       pub.publish(imgMsg)
       rospy.Rate(1.0).sleep()  # 1 Hz
   

7. ノードを実行して，新しくパブリッシュされた画像メッセージを調べてください．

   1. ノードの実行

      rosrun detect_pump image_pub.py ~/industrial_training/exercises/5.4/pump.jpg
      

   2. コマンドラインツールを用いたメッセージの調査

      rostopic list
      rostopic hz /image
      rosnode info /image_pub
      

   3. スタンドアロンの image_view ノードを用いたパブリッシュされた画像の調査

      rosrun image_view image_view
      

イメージ処理ノード Detect_Pump の作成

次のノードは image トピックを購読して， 一連の処理ステップを実行して， 画像水平軸に対するポンプの傾きを識別します．

1. 先程と同じように， 基本的な ROS の Python ノード（detect_pump.py）を作成して ファイルに実行権限を付与します．

   #!/usr/bin/env python
   import rospy
   
   # known pump geometry
   #  - units are pixels (of half-size image)
   PUMP_DIAMETER = 360
   PISTON_DIAMETER = 90
   PISTON_COUNT = 7
   
   def start_node():
       rospy.init_node('detect_pump')
       rospy.loginfo('detect_pump node started')
   
   if __name__ == '__main__':
       try:
           start_node()
       except rospy.ROSInterruptException:
           pass
   

   chmod u+x nodes/detect_pump.py
   

   • Python はコンパイルする必要がないので， 各スクリプトの新しいビルドルールを作成するために CMakeLists を編集する必要はありません．

2. image トピックを購読する ROS サブスクライバを追加して， 処理するための画像ソースを提供します．

   1. ヘッダ部分で Image メッセージをインポートします．

      from sensor_msgs.msg import Image
      

   2. start_node 関数の上に，新しい Image メッセージが受信されたときに呼び出される 空のコールバック（ process_image ）を作成します．

      def process_image(msg):
          try:
              pass
          except Exception as err:
              print err
      

      • try/except のエラー処理は，処理パイプライン中にエラーがあっても コードが実行され続けることを可能にします．

   3. start_node 関数内に ROS のサブスクライバ・オブジェクトを作成します．

      • image トピックを購読し，Image 型メッセージをモニタリングします．
      • 上で定義したコールバック関数を登録します．

      rospy.Subscriber("image", Image, process_image)
      rospy.spin()
      

      • 参考
        • rospy.Subscriber
        • rospy.spin

   4. 新しいノードを実行して， 意図したとおりにトピックを購読しているかを確認してください．

      rosrun detect_pump detect_pump.py
      rosnode info /detect_pump
      rqt_graph
      

3. 受信した Image メッセージを OpenCV Image オブジェクトに変換して表示します．

   前と同じように CvBridge モジュールを使って変換を行います．

   1. CvBridge モジュールをインポートします．

      from cv_bridge import CvBridge
      

   2. process_image コールバックの中で CvBridge の imgmsg_to_cv2 メソッドへのコールを追加します．

      # convert sensor_msgs/Image to OpenCV Image
      bridge = CvBridge()
      orig = bridge.imgmsg_to_cv2(msg, "bgr8")
      

      • このコード（および他のすべての画像処理コード）では， 処理エラーがノードをクラッシュさせないように try ブロックの中に入れると良いです．
      • これは先程 try ブロック内に仮に置かれた pass コマンドを置き換えます．

   3. 受信した画像を表示するには OpenCV の imshow メソッドを使用します． 各画像処理ステップの結果を表示するために再利用できるパターンを作成します．

      1. OpenCV cv2 モジュールをインポートします．

         import cv2
         

      2. process_image コールバックの前に表示補助の関数を追加します．

         def showImage(img):
             cv2.imshow('image', img)
             cv2.waitKey(1)
         

      3. 受信した画像を新しい "drawImg" 変数に代入します．

         drawImg = orig
         

      4. except ブロックの 下に （そのスコープの外側で） process_image スコープのあるところで drawImg 変数を表示します．

         # show results
         showImage(drawImg)
         

   4. ノードを実行して，受信した画像が表示されることを確認してください．

4. 画像処理パイプラインの最初のステップは， 画像のサイズを変更して将来の処理ステップを高速化することです． try ブロック内に次のコードを追加しノードを再実行してください．

   # resize image (half-size) for easier processing
   resized = cv2.resize(orig, None, fx=0.5, fy=0.5)
   drawImg = resized
   

   • 前よりも小さい画像が表示されるはずです．
   • 参考
     • resize()

5. 次に画像をカラーからグレースケールに変換します． ノードを実行してエラーをチェックしますが， 画像は以前と同じように見えるでしょう．

   # convert to single-channel image
   gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY)
   drawImg = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
   

   • 元の画像はグレーに見えますが JPG ファイル，Image メッセージ，および orig OpenCV イメージは すべて3チャネルのカラー画像です．
   • 多くの OpenCV の機能は個々の画像チャンネルで動作します． グレー表示の画像を「真の」1チャンネルグレースケール画像に変換すると， 取り違えを避けることができます．
   • 再変換をして drawImg のカラー画像に戻して， 画像の上に色付きのオーバーレイを描画し， 今後の処理ステップの結果を表示することができます．
   • 参考
     • cvtColor()

6. 閾値処理を適用してグレースケール画像を二値画像に変換します． ノードを実行して，二値化された画像を表示します．

   # threshold grayscale to binary (black & white) image
   threshVal = 75
   ret,thresh = cv2.threshold(gray, threshVal, 255, cv2.THRESH_BINARY)
   drawImg = cv2.cvtColor(thresh, cv2.COLOR_GRAY2BGR)
   

   threshVal パラメータをいろいろ試して， この画像に最も適した値を探してください． このパラメータの有効な値は [0-255] の間にあり， グレースケールのピクセル強度レンジと一致します． ポンプ面の形状を明確に強調する値を見つけます． 150 という値が良いようです．

   • 参考
     • threshold

7. 外側のポンプハウジングの円形状を検出します．

   これは実際にはポンプ角度を検出するためには使用されませんが，特徴検出の良い例となっています． より複雑なシーンでは OpenCV の関心領域（ROI）機能を使用して， このポンプハウジング円形状内の特徴だけに処理を制限することができます．

   1. HoughCircles メソッドを使用して，既知のサイズのポンプハウジングを検出します．

      # detect outer pump circle
      pumpRadiusRange = ( PUMP_DIAMETER/2-2, PUMP_DIAMETER/2+2)
      pumpCircles = cv2.HoughCircles(thresh, cv2.HOUGH_GRADIENT, 1, PUMP_DIAMETER, param2=2, minRadius=pumpRadiusRange[0], maxRadius=pumpRadiusRange[1])
      

      • 参考
        • HoughCircles

   2. 検出されたすべてのサークルを表示する関数を追加します．（ process_image コールバックの前 ）

      def plotCircles(img, circles, color):
          if circles is None: return
      
          for (x,y,r) in circles[0]:
              cv2.circle(img, (int(x),int(y)), int(r), color, 2)
      

   3. 円検出の後で表示機能を呼び出し予想される円の数を確認します．（1）

      plotCircles(drawImg, pumpCircles, (255,0,0))
      if (pumpCircles is None):
          raise Exception("No pump circles found!")
      elif len(pumpCircles[0])<>1:
          raise Exception("Wrong # of pump circles: found {} expected {}".format(len(pumpCircles[0]),1))
      else:
          pumpCircle = pumpCircles[0][0]
      

   4. ノードを実行して，円形状が検出されるのを確認してください．

      • HoughCircles への param2 入力を調整して， うまくいくと思われる値を見つけてみてください． このパラメータは検出器の感度を表します． 値を小さくするほど円が多く検出されますが，誤検知も多くなります．
      • min/maxRadius パラメータを削除するか， 円間の最小距離（4番目のパラメータ）を減らして， 他のサークルが検出されたことを確認します．
      • param2=7 あたりの値で良い具合に検出できるようです．

8. 連結領域（ブロブ）の検出を使用してピストン・スリーブを検出します．

   ブロブ検出は類似した色の連結領域（ブロブ）を識別するために画像を分析します． サイズや形状またはその他の特性に関する結果のブロブフィーチャのフィルタリングは， 関心のある特徴を識別するのに役立ちます． OpenCV の SimpleBlobDetector を利用します．

   1. 二値化した画像に対してブロブ検出を行うために次のコードを追加してください．

      # detect blobs inside pump body
      pistonArea = 3.14159 * PISTON_DIAMETER**2 / 4
      blobParams = cv2.SimpleBlobDetector_Params()
      blobParams.filterByArea = True;
      blobParams.minArea = 0.80 * pistonArea;
      blobParams.maxArea = 1.20 * pistonArea;
      blobDetector = cv2.SimpleBlobDetector_create(blobParams)
      blobs = blobDetector.detect(thresh)
      

      • 予想されるピストンスリーブ面積の 20％ 以内のブロブを選択するために エリアフィルタを使用することに注目してください．
      • デフォルトでは，ブロブ検出は白い背景上の黒いブロブを検出するように設定されています． そのため追加のカラーフィルタリングは必要ありません．

   2. ブロブ検出の後で OpenCV ブロブ表示機能を呼び出して， 期待されるピストン・スリーブ数（7）を確認します．

      drawImg = cv2.drawKeypoints(drawImg, blobs, (), (0,255,0), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
      if len(blobs) <> PISTON_COUNT:
          raise Exception("Wring # of pistons: found {} expected {}".format(len(blobs), PISTON_COUNT))
      pistonCenters = [(int(b.pt[0]),int(b.pt[1])) for b in blobs]
      

   3. ノードを実行して，すべてのピストン・スリーブが適切に識別されているかどうかを確認します．

9. ポンプボディの主軸を検出します．

   この軸はキー・ピストン・スリーブの特徴を識別するために使用されます． 画像を輪郭（アウトライン）に変換し， 次に最大の輪郭を見つけ， 長方形のボックスにフィットさせ（最適に回転させる）， そのボックスの長軸を特定します．

   1. 画像の輪郭を計算して，最大の面積を持つ輪郭を選択します．

      # determine primary axis, using largest contour
      im2, contours, h = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
      maxC = max(contours, key=lambda c: cv2.contourArea(c))
      

   2. 最大の輪郭にバウンディング・ボックスをフィットさせます．

      boundRect = cv2.minAreaRect(maxC)
      

   3. 次の3つの補助関数をコピー＆ペーストして（ process_image コールバックの上 ）， 長方形の主軸の端点を計算します．

      import math
      ...
      
      def ptDist(p1, p2):
          dx=p2[0]-p1[0]; dy=p2[1]-p1[1]
          return math.sqrt( dx*dx + dy*dy )
      
      def ptMean(p1, p2):
          return ((int(p1[0]+p2[0])/2, int(p1[1]+p2[1])/2))
      
      def rect2centerline(rect):
          p0=rect[0]; p1=rect[1]; p2=rect[2]; p3=rect[3];
          width=ptDist(p0,p1); height=ptDist(p1,p2);
      
          # centerline lies along longest median
          if (height > width):
              cl = ( ptMean(p0,p1), ptMean(p2,p3) )
          else:
              cl = ( ptMean(p1,p2), ptMean(p3,p0) )
      
          return cl
      

   4. 先に計算された境界矩形データを用いて， 上記の rect2centerline 関数を呼び出します． 表示画像上に中心線を描画します．

      centerline = rect2centerline(cv2.boxPoints(boundRect))
      cv2.line(drawImg, centerline[0], centerline[1], (0,0,255))
      

10. 最後のステップは（中心線に最も近い）キー・ピストン・スリーブを特定し， ポンプ角度を計算するためにその位置を用います．

    1. 点と中心線の間の距離を計算する補助関数を追加します．

       def ptLineDist(pt, line):
           x0=pt[0]; x1=line[0][0]; x2=line[1][0];
           y0=pt[1]; y1=line[0][1]; y2=line[1][1];
           return abs((x2-x1)*(y1-y0)-(x1-x0)*(y2-y1))/(math.sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)))
       

    2. ptLineDist 関数を呼び出すと，中心線に最も近いピストンのブロブを見つけることができます． どのブロブが識別されたかを示す drawImg を更新します．

       # find closest piston to primary axis
       closestPiston = min( pistonCenters, key=lambda ctr: ptLineDist(ctr, centerline))
       cv2.circle(drawImg, closestPiston, 5, (255,255,0), -1)
       

    3. 3つのキーとなる点間の角度を計算します．

       • ピストン・スリーブ中心点
       • ポンプ中心点
       • 水平軸に沿った任意の点（基準の「ゼロ」位置）

       1. 3点間の角度を計算する補助関数 findAngle を追加します．

          import numpy as np
          
          def findAngle(p1, p2, p3):
              p1=np.array(p1); p2=np.array(p2); p3=np.array(p3);
              v1=p1-p2; v2=p3-p2;
              return math.atan2(-v1[0]*v2[1]+v1[1]*v2[0],v1[0]*v2[0]+v1[1]*v2[1]) * 180/3.14159
          

       2. 適切なキーとなる3つの点を用いて findAngle 関数を呼び出します．

          # calculate pump angle
          p1 = (orig.shape[1], pumpCircle[1])
          p2 = (pumpCircle[0], pumpCircle[1])
          p3 = (closestPiston[0], closestPiston[1])
          angle = findAngle(p1, p2, p3)
          print "Found pump angle: {}".format(angle)
          

11. 全てを終えました！ 前と同じようにノードを実行します． 報告されたポンプ角度は 24° 付近になるはずです．

演習チャレンジ

より大きなチャレンジとして， 下記の提案を試してこの画像処理例の操作を変更してみてください．

1. パブリッシュ・ステップ間に画像を 10° 回転するように image_pub ノードを修正してください． 次のコードで画像を回転させることができます：

   def rotateImg(img, angle):
       rows,cols,ch = img.shape
       M = cv2.getRotationMatrix2D((cols/2,rows/2),angle,1)
       return cv2.warpAffine(img,M,(cols,rows))
   

2. detect_pump ノードを変更して， 画像検出を行う サービス を提供してください． 入力画像を取り込み，ポンプ角度を出力する，カスタムサービスタイプを定義します． image トピックを購読して， detect_pump サービスを呼び出す新しいアプリケーション・ノードを作成します．

3. BlobDetector の代わりに HoughCircles を使って ピストン・スリーブを探してみてください．

セッション 6 - ドキュメント生成 / ユニットテスト / ROS ユーティリティ / デバッグ

スライド

ドキュメント生成

モチベーション

ROS Scan-N-Plan アプリケーションを完成させテストを行いました． 他の開発者がこのプログラムを簡単に理解できるように， そのコードを完全に文書化することが重要です．

情報とリソース

• Doxygen - Generate documentation from source code
• rosdoc_lite is a ROS wrapper for doxygen

Scan-N-Plan アプリケーション: 演習問題

これまでに Scan-N-Plan プログラムを完成させテストを行いました． そして，そのコードを公開リリースする必要があります． 目標はブラウザでドキュメントを閲覧可能にすることです． これは、myworkcell_core パッケージに doxygen 構文で注釈を付けて， rosdoc_lite でドキュメントを生成することで実現可能です．

Scan-N-Plan アプリケーション: ガイダンス

ソースコードに注釈を付ける

1. 作成した myworkcell_node.cpp ファイルを開いてください．

2. ScanNPlan クラスの上に注釈を入れます．

   /**
    * @brief The ScanNPlan class is a client of the vision and path plan servers.  The ScanNPLan class takes
    * these services, computes transforms and published commands to the robot.
    */
   class ScanNPlan
   

3. start メソッドの上に注釈を入れます．

   /**
    * @brief start performs the robot alorithms functions of the ScanNPlan of
    * the node. The start method makes a service request for a transform that
    * localizes the part.  The start method moves the "manipulator"
    * move group to the localization target.  The start method requests
    * a cartesian path based on the localization target.  The start method
    * sends the cartesian path to the actionlib client for execution, bypassig
    * MoveIt!
    * @param base_frame is a string that specifies the reference frame
    * coordinate system.
    */
   void start()
   

4. flipPose の上に注釈を入れます．

   /**
    * @brief flipPose rotates the input transform by 180 degrees about the
    * x-axis
    * @param in geometry_msgs::Pose reference to the input transform
    * @return geometry_msgs::Pose of the flipped output transform
    */
   geometry_msgs::Pose transformPose(const geometry_msgs::Pose& in) const
   

5. main 関数の上に注釈を入れます．

   /**
    * @brief main is the ros interface for the ScanNPlan Class
    * @param argc ROS uses this to parse remapping arguments from the command line.
    * @param argv ROS uses this to parse remapping arguments from the command line.
    * @return ROS provides typical return codes, 0 or -1, depending on the
    * execution.
    */
   int main(int argc, char** argv)
   

6. 注釈を追加するときは プライベート変数や，その他重要なコード要素の上に置くと良いでしょう．

ドキュメントの生成

1. rosdoc_lite をインストールしてください．

   sudo apt install ros-kinetic-rosdoc-lite
   

2. パッケージをビルドしてください．

   catkin build
   

3. パッケージをソース（ source ）してください．

   source ./devel/setup.bash
   

4. rosdoc_lite を実行してドキュメントを生成します．

   roscd myworkcell_core
   rosdoc_lite .
   

ドキュメントの閲覧

1. ブラウザからドキュメントを開きます．

   firefox doc/html/index.html
   

2. Classes -> ScanNPlan でドキュメントを表示させてください．

ユニットテスト

この演習ではユニットテストを myworkcell_core パッケージに記述します．

モチベーション

ROS の Scan-N-Plan アプリケーションを完成し，文書化もしました． プログラムをテストして意図したとおりに動作することを確認します．

情報とリソース

• Google Test : C++ XUnit test framework
• rostest : ROS wrapper for XUnit test framework
• catkin testing : Building and running tests with catkin

演習問題

Scan-N-Plan プログラムを完成して文書化しました． ビルドした後にプログラムが意図したとおりに動作することを確認するために， テストフレームワークを作成します． ユニットテストでは コードが意図したとおりに実行されることを確認することに加えて 新しいコードが意図しない機能変更を生じるか否かを 簡単に確認することが可能になります．

目標はユニットテストのフレームワークを作成して， いくつかのテストを書くことです．

ガイダンス

ユニットテスト・フレームワークの作成

1. test フォルダを myworkcell_core/src の下に作成します． そしてワークスペース・ディレクトリで次のことを行ってください．

   catkin build
   source devel/setup.bash
   roscd myworkcell_core
   mkdir src/test
   

2. utest.cpp というファイルを myworkcell_core/src/test フォルダ内に作成します．

   touch src/test/utest.cpp
   

3. utest.cpp を QT などで編集して ros と gtest をインクルードします．

   #include <ros/ros.h>
   #include <gtest/gtest.h>
   

4. 実行すると true を返すダミーテストを作成します． これは後でフレームワークをテストし，より有用なテストに置き換えます．

   TEST(TestSuite, myworkcell_core_framework)
   {
     ASSERT_TRUE(true);
   }
   

5. 次に一般的な main 関数を書き込みます．

   この main 関数は後で記述するユニットテストを実行します．

   int main(int argc, char **argv)
   {
     testing::InitGoogleTest(&argc, argv);
     return RUN_ALL_TESTS();
   }
   

6. myworkcell_core の CMakeLists.txt を編集して， u_test.cppファイルをビルドします．

   CMakeLists.txt に次を追加してください．

   if(CATKIN_ENABLE_TESTING)
     find_package(rostest REQUIRED)
     add_rostest_gtest(utest_node test/utest_launch.test src/test/utest.cpp)
     target_link_libraries(utest_node ${catkin_LIBRARIES})
   endif()
   

7. myworkcell_core の下に test フォルダを作成してください．

   mkdir test
   

8. テスト launch ファイルを作成します．

   touch test/utest_launch.test
   

9. utest_launch.test ファイルを QT などで編集して， 次のコードを書き入れてください．

   <?xml version="1.0"?>
   <launch>
       <node pkg="fake_ar_publisher" type="fake_ar_publisher_node" name="fake_ar_publisher"/>
       <test test-name="unit_test_node" pkg="myworkcell_core" type="utest_node"/>
   </launch>
   

10. ビルドして，フレームワークをテストします．

    catkin run_tests myworkcell_core
    

    コンソール出力では次のように表示されるはずです． （ 多くのビルドメッセージの中に埋もれています．）

    [ROSTEST]-----------------------------------------------------------------------
    
    [myworkcell_core.rosunit-unit_test_node/myworkcell_core_framework][passed]
    
    SUMMARY
     * RESULT: SUCCESS
     * TESTS: 1
     * ERRORS: 0
     * FAILURES: 0
    

    これはフレームワークが機能し，ユニットテストを追加することができる状態であることを意味します．

    注釈

    上記の launch ファイルを使用して， コマンドラインから直接テストを実行することもできます． rostest myworkcell_core utest_launch.test テストファイルは通常の catkin build コマンドを使って作られていないので， 代わりに catkin run_tests myworkcell_core を使ってください．

ストック・パブリッシャ・テストの追加

1. rostest パッケージには基本的なトピックの特性を検査するためのいくつかのツールがあります． hztest や paramtest ， publishtest です． fake_ar_publisher ノードが 期待するトピックを出力していることを検証するための 基本的なテストをいくつか追加します。

2. 次のテストの記述を utest_launch.test ファイルに追加してください．

   <test name="publishtest" test-name="publishtest" pkg="rostest" type="publishtest">
       <rosparam>
         topics:
           - name: "/ar_pose_marker"
             timeout: 10
             negative: False
           - name: "/ar_pose_visual"
             timeout: 10
             negative: False
       </rosparam>
   </test>
   

3. テストを実行します．

   catkin run_tests myworkcell_core
   

   次のような表示がされるはずです．

   Summary: 2 tests, 0 errors, 0 failures
   

特定のユニットテストの記述

1. fake_ar_publisher パッケージから取得したメッセージをテストしたいので， 関連するヘッダファイルを（ utest.cpp に ）インクルードしてください．

   #include <fake_ar_publisher/ARMarker.h>
   

2. グローバル変数の宣言

   fake_ar_publisher::ARMarkerConstPtr test_msg_;
   

3. 受信したメッセージをグローバル変数にコピーするためのサブスクライバ・コールバックを追加します．

   void testCallback(const fake_ar_publisher::ARMarkerConstPtr &msg)
   {
     test_msg_ = msg;
   }
   

4. ar_pose_marker の参照フレームをチェックするユニットテストの記述

   TEST(TestSuite, myworkcell_core_fake_ar_pub_ref_frame)
   {
       ros::NodeHandle nh;
       ros::Subscriber sub = nh.subscribe("/ar_pose_marker", 1, &testCallback);
   
       EXPECT_NE(ros::topic::waitForMessage<fake_ar_publisher::ARMarker>("/ar_pose_marker", ros::Duration(10)), nullptr);
       EXPECT_EQ(1, sub.getNumPublishers());
       EXPECT_EQ(test_msg_->header.frame_id, "camera_frame");
   }
   

5. ユニットテストが実行中の ROS システムとやりとりするので， main() 関数にノード初期化用定型文を追加してください． 現在の main() 関数を次の新しいコードに置き換えます．

   int main(int argc, char **argv)
   {
     testing::InitGoogleTest(&argc, argv);
     ros::init(argc, argv, "MyWorkcellCoreTest");
   
     ros::AsyncSpinner spinner(1);
     spinner.start();
     int ret = RUN_ALL_TESTS();
     spinner.stop();
     ros::shutdown();
     return ret;
   }
   

6. テストを実行します．

   catkin run_tests myworkcell_core
   

7. テスト結果の表示

   catkin_test_results build/myworkcell_core
   

   次のように表示されるはずです．

   Summary: 3 tests, 0 errors, 0 failures
   

rqt 分析ツール

本演習では ROS システムの動作を理解するために rqt_graph , rqt_console , rqt_plot を使ってみます．

モチベーション

複雑なマルチノードの ROS システムが動作している場合は， ノードの相互作用を理解することが重要になります．

情報とリソース

• Building and using catkin packages in a workspace
• workspace で catkin パッケージをビルドして使う

演習問題

Scan-N-Plan アプリケーションは完了しました． さまざまな ROS rqt ツールを用いてアプリケーションをさらに検査したいと考えています．

ガイダンス

rqt_graph : ノードの相互作用状況の表示

複雑なアプリケーションでは ROS ノードの相互作用の状況を視覚的に表示すると便利です．

1. Scan-N-Plan ワークセルを起動します．

   roslaunch myworkcell_support setup.launch
   

2. 2つ目のターミナルで rqt_graph を起動します．

   rqt_graph
   

3. これで Scan-N-Plan アプリケーションの基本的なレイアウトを見ることができます．

   

4. 3つ目のターミナルで直交座標系動作軌道プランナを実行します．

   rosrun myworkcell_core myworkcell_node
   

5. グラフが自動的に更新されないため，ノードの実行中にグラフを更新する必要があります．

   更新すると ROS ネットワークに myworkcell_node が含まれている表示がされるはずです． また，myworkcell_node は move_group ノードによって購読される 新しいトピック /move_group/goal をパブリッシュしています．

   

rqt_console : メッセージの表示

動作軌道プランナの出力を見たいと思います． rqt_console は ROS トピックを表示するための優れた GUI です．

1. 2つ目のターミナルで rqt_graph アプリケーションを停止して rqt_console を実行します．

   rqt_console
   

2. 動作軌道プランナを実行します．

   rosrun myworkcell_core myworkcell_node
   

3. rqt_console は自動的に更新されます．

   次のように動作軌道プランナの背後にあるロジックが表示されるはずです．

   

rqt_plot: データのプロット表示

rqt_plot は ROS データをリアルタイムでプロットする簡単な方法です． ここの例では動作軌道計画からロボットの関節角度をプロットします．

1. 2つ目のターミナルで rqt_console アプリケーションを停止して rqt_plot を実行します．

   rqt_plot
   

2. Topic 欄に下記のトピックを追加します．

   /joint_states/position[0]
   /joint_states/position[1]
   /joint_states/position[2]
   /joint_states/position[3]
   /joint_states/position[4]
   /joint_states/position[5]
   

3. 動作軌道プランナを実行します．

   rosrun myworkcell_core myworkcell_node
   

4. 関節角度がリアルタイムでストリーミングされ，プロットされているはずです．

   

ROS スタイルガイドと ros_lint

モチベーション

ROS Scan-N-Plan アプリケーションは完成し，テストされ，文書化されています． 他の開発者がコードを簡単に理解できるように， スタイルガイドに従ってコードをクリーンアップする必要があります．

情報とリソース

• ROS C++ Style Guide
• Automated Style Guide Enforcement
• ROS C++ スタイルガイド

Scan-N-Plan アプリケーション: 演習問題

これまでに Scan-N-Plan プログラムを完成させ，テストもしました． そのコードを公開する必要があります．

目標は作成したコードが ROS C ++スタイルガイド に準拠していることを確認することです．

Scan-N-Plan アプリケーション: ガイダンス

パッケージの設定

1. パッケージの package.xml に roslint のビルド依存関係を追加します．

   <build_depend>roslint</build_depend>
   

2. CMakeLists.txt の catkin REQUIRED COMPONENTS に roslint を追加します．

   find_package(catkin REQUIRED COMPONENTS
     ...
     roslint
   )
   

3. CMakeLists.txt から roslint 関数を呼び出します．

   roslint_cpp()
   

roslint の実行

1. roslint を実行します．

   roscd myworkcell_core
   catkin_make --make-args roslint
   

ROS と Docker / Amazon Web Service

デモ #1 - フロントエンド: ホスト Gazebo / バックエンド: Docker

ワークスペースのセットアップ

フロントエンド（ ホストで実行 / GUI のみこちら ）

1つ目のターミナルで次を実行してください．

mkdir -p ./training/front_ws/src
cd ./training/front_ws/src
gazebo -v
git clone -b gazebo7 https://github.com/fetchrobotics/fetch_gazebo.git
git clone https://github.com/fetchrobotics/robot_controllers.git
git clone https://github.com/fetchrobotics/fetch_ros.git
cd ..
catkin build fetch_gazebo fetch_description

バックエンド（ Docker コンテナ内で実行 ）

このステップでは必要な実行可能ファイルを含む Docker イメージを作成します．

• rosindustrial/core:indigo イメージ内で /bin/bash を実行してから apt-get でパッケージを取得

• rosindustrial/core:indigo イメージ内で /bin/bash を実行してから ソースからパッケージをビルドして，その結果をコミット

• fetch ロボットの Dockerfile で Docker コンテナを作成し，それを実行 https://gist.github.com/AustinDeric/242c1edf1c934406f59dfd078a0ce7fa

  cd ../fetch-Dockerfile/
  docker build --network=host -t rosindustrial/fetch:indigo .
  

デモンストレーションの実行

フロントエンドの実行

1つ目のターミナルからフロントエンドを実行します．

source devel/setup.bash
roslaunch fetch_gazebo playground.launch

バックエンドの実行

これを実行するには複数の方法があります．

• fetch コンテナ内で /bin/bash を実行し，デモノードを手動で実行する方法
• デモノードをコンテナ内で直接実行する方法（ これから本演習で行います ）

2つ目のターミナルで次のとおりバックエンドを実行してください．

docker run --network=host rosindustrial/fetch:indigo roslaunch fetch_gazebo_demo demo.launch

デモ #2 - フロントエンド: Web サーバ / バックエンド: Docker

環境を開始します．

docker run --network=host rosindustrial/fetch:indigo roslaunch fetch_gazebo playground.launch headless:=true gui:=false

Gazebo の Web サーバを実行します．

docker run -v "/home/aderic/roscloud/training/front_ws/src/fetch_gazebo/fetch_gazebo/models/test_zone/meshes/:/root/gzweb/http/client/assets/test_zone/meshes/" -v "/home/aderic/roscloud/training/front_ws/src/fetch_ros/fetch_description/meshes:/root/gzweb/http/client/assets/fetch_description/meshes" -it --network=host giodegas/gzweb /bin/bash

サーバを実行します．

/root/gzweb/start_gzweb.sh && gzserver

3つ目のターミナルでデモを実行します．

docker run --network=host fetch roslaunch fetch_gazebo_demo demo.launch

デモ #3 ロボット Web ツール

このデモでは産業用ロボットURDFをブラウザで表示することができます．

1つ目のターミナルで，ロボットをパラメータサーバに読み込ませます．

mkdir -p abb_ws/src
git clone -b kinetic-devel https://github.com/ros-industrial/abb.git
docker run -v "/home/aderic/roscloud/training/abb_ws:/abb_ws" --network=host -it rosindustrial/core:kinetic /bin/bash
cd abb_ws
catkin build
source devel/setup.bash
roslaunch abb_irb5400_support load_irb5400.launch

2つ目のターミナルで，ロボット Web ツールを開始します．

docker run --network=host rosindustrial/viz:kinetic roslaunch viz.launch

3つ目のターミナルで Web サーバを起動します． まず www フォルダを開始する必要があります．

cp -r abb_ws/src/abb/abb_irb5400_support/ www/

docker run -v "/home/aderic/roscloud/training/www:/data/www" -v "/home/aderic/roscloud/training/nginx_conf/:/etc/nginx/local/" -it --network=host rosindustrial/nginx:latest /bin/bash
nginx -c /etc/nginx/local/nginx.conf