【ROS】robot_self_filter の使い方【初心者向け】

# ロボットのリンクをカメラ入力点群から除去
ヒューマノイドロボット+ビジョンを専攻している博士学生の鶴です。

PR2やFetchなど、カメラ視野にロボット自身のアームや手先が映ると障害物回避計画で困りますよね。
robot_self_filter (http://wiki.ros.org/robot_self_filter) を使えば、ロボットのURDFモデルとTFフレームを参照してリアルタイムで点群からロボットリンクを除去できます。

↓before
![Screenshot from 2022-07-01 13-41-31.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/103136/5d60f1a7-001a-d724-5f3e-c82f6a7a5eae.png)
↓after
![Screenshot from 2022-07-01 13-41-46.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/103136/faaa538a-ae5b-e246-c7f1-3a0cd242c91a.png)

点群を白色で表示しています。画像の通り、両手の指の点群が消えました！


ただ、ドキュメントが本当に ~~ゴミカス~~ 乏しく、公式launchファイルすら存在しないので、とにかく初心者には使いづらい…
仕方ないので、私が使い方の記事とlaunchファイルを提供します。

学部4年生あたりのROS初心者をターゲットに、丁寧に書きます。

質問コメントも即リプを心がけますのでお気軽に。
自分の研究に使うよって方は是非コメント欄かTwitterDM下さい。色々とアシストします。
(私もROSが分かる先輩が周囲におらず、孤独にROSを学んだ者なので辛さは分かります。)

# 1分解説


結論から書きます。この4行で済みます。


1. `$ sudo apt install ros-melodic-robot-self-filter`
2. 下のlaunchファイルとyamlファイルをダウンロードして、自分のパッケージにコピペ
3. yamlファイルに除去したいリンクの名前を羅列
4. launchファイルを実行 (`$ roslaunch 皆様の自作パッケージ名 robot_self_filter.launch`)

```robot_self_filter.launch
<launch>
 <node pkg="robot_self_filter" type="self_filter" name="robot_self_filter">
    <remap from="cloud_in" to="皆様のカメラ入力点群のトピック名"/>
    <remap from="cloud_out" to="/self_filtered_cloud" />

    <param name="sensor_frame" type="string" value="入力点群のTFフレーム名"/>    
    <!-- <param name="sensor_frame" type="string" value="left_camera_optical_frame"/> -->
    
    <param name="use_rgb" type="bool" value="true"/>
    <rosparam command="load" file="$(find 皆様の自作パッケージ名)/launch/remove_links.yaml"/>
  </node>
</launch>
```

```remove_links.yaml
self_see_links:
  - name: Larm_Link5
    padding: .03
    scale: 1.0
  - name: Larm_Link6
    padding: .03
    scale: 1.0
  - name: Lhand_Link0_Plan2
    padding: .03
    scale: 1.0
  - name: Lindex_Link0
    padding: .03
    scale: 1.0
  - name: Lindex_Link1
    padding: .03
    scale: 1.0
  - name: Lindex_Link2
    padding: .03
    scale: 1.0
```


yamlファイルに書くリンク名はRVizに表示されるロボットモデルと同一です。
![Screenshot from 2022-07-01 13-42-16.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/103136/6ef7306f-2fe9-2d62-dd23-3eef728f316f.png)

(簡易化のため今回は左手だけにしています。右手も消したい場合は、同様にリンク名をyamlファイルに追記するだけで除去できます。)
(launchファイル内の`<node> </node>`タグで囲まれた部分を皆様のlaunchファイルにコピペしても使えます。)

# 説明

## 1. apt経由でrobot_self_filterをインストールしよう！

ターミナルを開いて以下のコマンドを実行しましょう。
`$ sudo apt install ros-melodic-robot-self-filter`

今回は私がUbuntu18.04 + ROS Melodicを使っているため、コマンド中に _melodic_ とあります。

もし皆様がKinetic(Ubuntu16.04)やNoetic(Ubuntu20.04)を使っている場合は、この部分を
`$ sudo apt install ros-kinetic-robot-self-filter`
`$ sudo apt install ros-noetic-robot-self-filter`
のように各バージョンに変更して実行してくださいね！

この`sudo apt install ros-バージョン名-パッケージ名`はあらゆるROSパッケージに共通する基本なので、必ず覚えましょう。

_Tips:
そもそもapt installとは、ソースコード(.cpp)をダウンロード・コンパイルせず、機械語に翻訳・完成された形式でソフトウェアをダウンロードする方法です。コードの部分的改造ができない代わりに、コンパイルの手間が省けるメリットがあります。aptで配布されているROSパッケージは極力apt版を使いましょう。どうせ初心者はコンパイルで詰みますので。_

## 2. launchファイルと設定ファイル

皆様、launchファイルを書くのを後回しにしてませんか？絶対にlaunchファイルは書きましょう。
理由は、本件の執筆に至った原因 __『robot_self_filterに公式launchファイルが無い問題』__ が全てを体現しています。
launchファイルが無いと一般人はどうやって起動したらいいのか理解できないのです。 ~~あなたもカス開発者の仲間入りです~~   
__robot_self_filter最高！うおおぉぉぉぉぉ！__
(rosrun+引数で動かしていては、1ヶ月後にはあなた自身もコマンドを忘れていますよ)

さて、先ほどのlaunchファイルの詳細を解説しましょう。


```robot_self_filter.launch
<launch>
 <node pkg="robot_self_filter" type="self_filter" name="robot_self_filter">
    <remap from="cloud_in" to="皆様のカメラ入力点群のトピック名"/>
    <remap from="cloud_out" to="/self_filtered_cloud" />

    <param name="sensor_frame" type="string" value="入力点群のTFフレーム名"/>    
    <!-- <param name="sensor_frame" type="string" value="left_camera_optical_frame"/> -->

    <param name="use_rgb" type="bool" value="true"/>
    <rosparam command="load" file="$(find 皆様の自作パッケージ名)/launch/remove_links.yaml"/>
  </node>
</launch>
```
### 2-1. `cloud_in`が除去フィルターの点群入力トピック
`cloud_in`が除去フィルターの点群入力トピックになります。形式はsensor_msgs::PointCloud2です。
remap機能で皆様のカメラトピックへ変更しましょう。
RealSenseシリーズなら`/camera/depth_registered/points`や、AuzreKinectなら`/k4a/points2`でしょうか。
自分の点群トピック名が分からないよって人は、カメラノードを立ち上げた後に`$ rostopic list`コマンドやRVizでPointCloud2形式のトピックを探しましょう。
ターミナルで`rostopic info 皆様のカメラトピック名 `とコマンドを打って通信メッセージ形式(Type)がPointCloud2かどうか確認すると更に確実です↓
![Screenshot from 2022-06-30 17-38-30.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/103136/2a8633a6-2ce3-4b00-589e-084956013623.png)

### 2-2. `cloud_out`が出力点群トピック
入力同様、サンプルのlaunchファイルでは`/self_filtered_cloud`にremapしています。
つまり、ロボットリンクが除去された後の点群は/self_filtered_cloudに出力されていますので、RVizではここを描画しましょう。
点群が映らないよって初心者はRVizのGlobal Fixed Frameの設定を忘れてるかも。
RVizのGlobalOptiions→FixedFrameから、RVizの視点基準となるTFフレームを指定しましょう！
### 2-3. `sensor_frame`を手動で設定する必要があるらしい
 ~~は？subscribeした点群メッセージのヘッダ情報から自動取得しろやks~~ 
どうやら大人の事情で皆様の手動設定が必要なようです。
例えばRealSenseなら`rostopic echo /camera/depth_registered/points/header`のように、トピック名の後ろに/headerを付ければheader情報だけ表示できるので非常に便利です。
(もし/headerを語尾に付けないと、点群データなら大変なことに…)
### 2-4. yamlファイルに記載した内容はros paramに行きます
`<rosparam command="load" file="$(find 皆様の自作パッケージ名)/launch/remove_links.yaml"/>`でyamlファイルをロードしています。除去したいリンク名はROSparamに一旦投影されています。
  launchファイル起動後に別ターミナルでrosparamの確認をした際に、こんな感じにyamlでの設定が映れば成功です！
![Screenshot from 2022-06-30 17-44-26.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/103136/6a026c7d-e34d-ae0a-1d07-bdb2862fde45.png)


# その他雑記

今回はロボットの3Dモデルを添えたサンプルを提供できませんでした、ごめんなさい…
(なにせヒューマノイドロボットのモデルは大人の事情で一般配布が禁止されているので…)
皆様のご愛用のアームロボットにご利用頂けると幸いです。

今回はrobot_self_filterを紹介しましたが、そっくりなrobot_body_filterというrosパッケージも存在しています。
しかし、なんだか特殊な形式をしており、ROSノード単体としては動作しません。結構面倒です。
コードを書く時は他人が実行しやすいよう、とにかく直感的な形式や処理をし、マイナーな3rdパーティーライブラリは使用を避けて、launchファイルを用意することを意識しましょう。

この記事が誰かの約に立つことを願っています。