Mycobot PiのMoveIt対応


# この記事の内容について
こんにちは、ROS Advent Calenderの8日目を担当させていただく株式会社キビテク開発部のAlirezaです。
こちらは[myCobot Pi](https://www.elephantrobotics.com/en/mycobot-pi/)でMoveItを使用する方法について説明させていただきます。 
[myCobot Pi](https://www.elephantrobotics.com/en/mycobot-pi/)はメーカーがMoveItのサポートを行っていません。myCobotのFacebookグループでは、  [myCobot Pi](https://www.elephantrobotics.com/en/mycobot-pi/)でMoveItを使用する方法を探している人が多くいました。
そこで、myCobot PiでMoveItが利用できず、困っている方のためにこの記事を書きました。

### このROSパッケージの簡単な説明 
* myCobot PiでMoveItが利用できるようにメーカーのROSパッケージ・3rd party ROSパッケージのソースコードを改変しました
* ROSでグリッパー（吸引タイプ、通常タイプ）を利用できるようにソースコードを追加しました



# myCobotの簡単な紹介
myCobotは、世界最小かつ最軽量の協調ロボットマニピュレータです。
$40000のマニピュレータと比較すると、myCobotは非常に安い価格です。
 [myCobot Pi](https://www.elephantrobotics.com/en/mycobot-pi/)はRaspberry Pi 4Bが搭載されているmyCobotロボットシリーズの一種です。

<div style="text-align:center"><img width="500" alt="Screenshot from 2021-11-13 15-15-39.png (327.1 kB)" src="https://img.esa.io/uploads/production/attachments/12680/2021/11/13/116092/01cd26fa-d9f0-4b43-8527-5fc60c6458ea.png"><figcaption align = "center">Fig.1 -    myCobot Pi </figcaption> </div>

# myCobot PiでMoveItを使用することが難しい理由
1.  [myCobot Pi](https://www.elephantrobotics.com/en/mycobot-pi/) にはROS Kineticが構築済みのRaspbian Buster OSがインストールされています。
このような組み込みOS向けのLinuxディストリビューションでは、MoveItをソースインストールする必要があります。
単純なバイナリインストールに比べて時間と労力がかかります。
MoveItをインストールした後でも、moveit rviz pluginの可視化処理が重すぎるため、RPiで処理できません。
そのため、PCとRPiのマルチマシン環境でROSを動作させることで、MoveItが動作するようにしました。（MoveIt Planner自体はRPiで処理可能です。）

2. MoveItを実際のロボットに対応するのは難しいです。
MoveItを実行するために必要最低限な3つの要件：
	1. /joint_state ROS Topic
	2. ロボットのtf情報
	3. followJointTrajectory Action Service

     myCobot Piのpkgを変更することで、1/2の要件を達成しました。
    （myCobot PiをROSで動作させる方法については、この会話の最後にあるメモを参照してください）。
    3の要件を達成するには、ros_controlを使用する必要があります。
   ロボットからのデータの読み取り、コントローラーへのデータの受け渡し、コントローラーからのコマンドの取得、およびロボットへのコマンドの送信を担当する"RobotHW"ノードも必要です。このROS NodeはRobot APIやモータ指令値等ににアクセスできるものです。

    ※Gazeboの場合、このノードを自分で作成する必要はなく、代わりに"Transmission Element"をジョイントに追加し、"ros_control"プラグインをurdfファイルに追加します。

# myCobot PiでMoveItを使用する
Mycobot PiでMoveItを使用する手順について説明します。 
Mycobot Piは購入時に、ROS KineticがインストールされたDebian Busterが構築済みです。
ここでは、ROS KineticがMyCobotPiにすでにインストールされていることを前提とします。

## 1. 必要なもの
1. myCobot Pi
2. ROS Kinetic/Melodic/Noetic構築済みのPC
MoveItを動作させるために必要です。
ROS Melodic/Noeticを利用することを推奨します。
PC側のROS Melodic/NoeticとRPi側のROS Kineticの組み合わせで動作確認しました。

3. ルーター
myCobotとPCは同一ネットワーク上にある必要があります。

図2に、システムの大まかな構成図を示します。

<div style="text-align:center"><img width="750" alt="Screenshot from 2021-11-13 15-15-39.png (327.1 kB)" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2252995/5e9e6326-30bb-3905-5827-ecb239294113.png"><figcaption align = "center">Fig.2 - 大まかなシステムの構成図 </figcaption> </div>


## 2. マルチマシンでのROS設定

myCobot PiでMoveItを使用するには、ロボットと同一ネットワーク内にある別のROS Melodic/Noetic構築済みPCが必要です。
 myCobotPiとコンピューターの両方がお互いを認識できることを確認してください。

* 次のコマンドでIPアドレスを取得できます。

  ```bash
  hostname -I
  ```

  まず、次のコマンドを使用して、rpiとコンピューターの両方がお互いを認識できることを確認してください。
    
  ```bash
  ping THE_OTHER_SYSTEM_IP
  ```
  THE_OTHER_SYSTEM_IP：他のシステム（rpiまたはPC）のIPアドレスです。

### ROS_MASTERの設定
マルチマシンでのROS動作時にはROS_MASTERの設定が必要です。

* PC側の実行コマンド

```bash
    export ROS_MASTER_URI=http://PC_IP_ADDRESS:11311
    export ROS_IP=PC_IP_ADDRESS
```
* RPi側の実行コマンド

```bash
    export ROS_MASTER_URI=http://PC_IP_ADDRESS:11311
    export ROS_IP=RPI_IP_ADDRESS        
```
これらのコマンドは、ターミナルを開くたびに実行してください。
### roscoreを実行する
PC側で

```bash
roscore
```
を起動してください。

詳細については、[こちら](http://wiki.ros.org/ROS/Tutorials/MultipleMachines)を参照してください.
## 3. MoveItのインストール
PC側にMoveItをインストールする必要があります。 
以下のコマンドを実行します。

```bash
sudo apt-get install ros-noetic-moveit
```

## 4. GitHubレポジトリのクローン
catkin workspaceにGithubリポジトリをクローンします。

  * RPI側での実行コマンド
 
  ```bash
    git clone -b  mycobot-pi https://github.com/QibiTechInc/mycobot_ros.git
    git clone -b rpi-pkg  https://github.com/QibiTechInc/mycobot_moveit.git

  ```

  * PC側の実行コマンド
  
    ```bash
    git clone -b  mycobot-pi https://github.com/QibiTechInc/mycobot_ros.git
    git clone -b pc-pkg https://github.com/QibiTechInc/mycobot_moveit.git
    ```
## 5.  RPI側でROS Launchを起動
RPi側で、以下のコマンドを実行。

```bash
 roslaunch mycobot_moveit mycobot_rpi_control_v2.launch
```
このROS Launchでは、以下の処理を行います。

* robot_descriptionパラメータの定義
*  robot_state_publisher Nodeの実行
* get / set_joint_angles ROS Serviceを管理するNodeの実行
* mycobot_moveitパッケージのmycobot_hw_interface.pyの実行
このNodeは、PC側で実行されるhw_interface.cpp Nodeの /joint_cmd_array Topicを購読し、指令値が配信されると、mycobot python APIを介してロボットにコマンドを送信します。

## 6. PC側でROS Launchを起動
コンピューターで以下のコマンドを実行。

```bash
roslaunch mycobot_moveit mycobot_moveit_control_master_v2.launch
```
このROS Launchでは、以下の処理を行います。

* moveitを実行するために必要なNodeの実行（moveit_ros_move_groupからのmove_group）
* ros controllerの生成（joint_state_publisherおよびfollow_joint_trajectory）
* mycobot_moveitパッケージのmycobot_control Nodeの実行。Controller Managerノード。

:tada: 上記の手順に従うことで、myCobotPiでMoveItを使用できるようになります :tada: 

 [![mycobot Pi with MoveIt](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2252995/68885364-3fcb-52a1-d4e8-bc37ecbd324b.png)](https://streamable.com/mea014)



# エンドエフェクタ
吸引ポンプの接続方式がわかりにくいので、説明します。
MyCobot Piには、通常の並行リンクのグリッパーと吸引ポンプがあります。

<div style="text-align:center"><img width="300" alt="photo_2021-11-13_10-21-18.jpg (132.4 kB)" src="https://img.esa.io/uploads/production/attachments/12680/2021/11/13/116092/e677f039-edc4-498f-99bd-2959e5092473.jpg">
<figcaption align = "center">Fig.3 - Current myCobot grippers</figcaption> </div>

図3では、上部に吸引ポンプがあり、右下にもう1つのグリッパーがあります。 並行リンクのグリッパーの接続方法については[こちらのリンク](https://www.elephantrobotics.com/docs/myCobot-en/3-development/7-side_products/7.1.1-gripper_ag.html)を参照ください。
吸引ポンプに関しては、モータとソレノイドが入っている小さな箱の中に、IOポートが2つあります。

<div style="text-align:center"><img width="300" alt="photo_2021-11-13_10-44-07.jpg (81.7 kB)" src="https://img.esa.io/uploads/production/attachments/12680/2021/11/13/116092/7db89914-8c76-4fb2-a4f3-3d5a5026c125.jpg"><figcaption align = "center">Fig.4 - suction pump power cable, and input command interfaces</figcaption> </div>

図4の左側のポートは吸引ポンプに電力を供給するために使用され、
右側のポートはG2ピンとG5ピンが吸引ポンプのON/OFFを指令するために利用されます。
 G2とG5をRaspberry PiのGPIOに接続することで、吸引ポンプのON/OFFを切り替えます。

### ROSで通常のグリッパーを使用する
次のパッケージを起動します。

```bash
roslaunch mycobot_280 gripper.launch  
```
これにより、ROSで並行リンクのグリッパーが利用できます。

### グリッパーの操作
グリッパーを開くように命令するには、 `/ gripper_command` ROS Topicを使用します。(std_msgs/Float64) 

0の指令値を送ると、グリッパーは完全に閉じます。
100の指令地を送ると、グリッパーは完全に開きます。

0~100の間の指令値は正確に動作しない場合があります。

### グリッパーの状態取得
ROS Topic `/ gripper_status`を介して、グリッパーの現在の状態（0〜100の数値）を取得できます。
このトピックでは、実際の値の代わりに-1の値が取得され、グリッパーの状態が取得できないことがあります。これは、mycobot APIの問題のようです。
※joint_anglesを読んでいる場合、mycobot.launchまたはMoveItを起動している場合に発生しやすいです。

### 吸引ポンプの設定（ハードウエア）
G2 : RPi GPIO 5ピン
G5 : RPi GPIO 6ピン
上記のように接続してください。

### ROSでmyCobot Piの吸引ポンプを動かしてみる

 [![mycobot Pi with MoveIt](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2252995/c02d14ce-cdfa-0e89-dd6a-4cf39ef73665.png)](https://streamable.com/5e0gd7)

以下のコマンドを実行します。

```bash
rosrun mycobot_280 suction_gripper.py 
```

std_msgs :: Bool型のメッセージを `/suction_gripper_command`トピックに送ることで、吸引グリッパーをON/OFFできます。

true : 吸引ポンプをON
false : 吸引ポンプをOFF

RPiのピン番号を変更したい場合は、suction_gripper.py内のG2_PINとG5_PINを任意のピン番号の値に変更してください。
# トラブルシューティング
プログラム内のパラメータのPORTとBAUD_RATEがmyCobotPiに対して適切に定義されていることを確認してください。現在のmyCobotPiの場合、この値は次のとおりです：

* BAUD_RATE = 1000000
* PORT = /dev/ttyAMA0

ポートのデバイスファイル名は以下のコマンドで発見できます。

```bash
    sudo ls /dev | grep "tty"
```
* ポートが閉じられているエラーが出た場合、以下のコマンドを使用してRPiのシリアルポートのユーザー権限を付与してください。

 ```bash
   sudo chmod 777 /dev/ttyAMA0
 ```

### mycobot PIでmycobot Python APIを利用する場合
元のパッケージのプログラムの多くは、mycobotオブジェクトを定義するために以下のコンストラクタが使用されています。この形式を使用しないでください。

 ```python
    from pymycobot.mycobot import MyCobot
    
    mycobot = MyCobot(port)
 ```

上記の代わりに、以下のコンストラクタを使用してください。

```python
    from pymycobot.mycobot import MyCobot
    from pymycobot import PI_PORT, PI_BAUD
    
    mycobot_ = MyCobot(PI_PORT, PI_BAUD)
```

### ROSパッケージが見つからない場合
RPi側で``roslaunch``を使用してmycobot PiのROSパッケージが見つからなかった場合は、
以下のコマンドを利用してください。

```bash
 source ros_catkin_ws/devel/setup.bash
```


# 謝辞
 [Elephantrobotics](https://github.com/elephantrobotics/mycobot_ros)と[Nisshan](https://github.com/nisshan-x/mycobot_moveit)の功績を称えたいと思います。
これらのレポジトリ2つをforkし,  mycobot Piで利用できるように改変しました。