ROSの勉強　第43弾：Dynamixel

# #プログラミング ROS< Dynamixel >

## はじめに
ROS(Robot Operating System)をさらに扱えるようになることが目的である．その第43弾として，「Dynamixel」を扱う．

## 環境
##### コンピュータ
|||
|:-:|:--|
|デバイス|raspberry pi 4B+|
|実装RAM|4.00 GB |
|OS|ubuntu-mate-20.04.1-desktop-amd64|

##### ROS
|||
|:-:|:--|
|Distribution|noetic|

##### Dynamixel
|||
|:-:|:--|
|メーカー|ROBOTIS|
|シリーズ|XM430-W350-R|
|仕様詳細|https://emanual.robotis.com/docs/en/dxl/x/xm430-w350/|

###### ロボット
|||
|:-:|:--|
|メーカー|ROBOTIS|
|シリーズ|OpenMANUPILATOR-X|
|仕様詳細|https://emanual.robotis.com/docs/en/platform/openmanipulator_x/overview/|

![DSC_0604.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/56e3e5a5-824b-310f-0a53-a46902bbc8b7.jpeg)


## DynamixelSDK
Dynamixelを扱うためのパッケージ
### 環境構築
#### 手順①：ワークスペースのsrcへ移動
```
~/catkin/src 
```
#### 手順②：gitを使ってクローン作成
```
git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git
```
#### 手順③：ワークスペースへ移動
```
cd ..
```
#### 手順④：DynamixelSDKをROSに適用
```
catkin_make
```
ここまでの操作を終えれば，dynamixel_sdkとdynamixel_sdk_examplesに関するライブラリが利用可能となる．なお，cppとpythonの両方の言語のプログラムが用意されている．それらを参照することで使い方を学ぶことができる． 

### dynamixel_sdk_examplesのメッセージ
dynamixel_sdk_examplesは3つのメッセージを持つ 

* SetPosition 

```
uint8 id 
int32 position 
```
 
* SyncSetPosition 

```
uint8 id1 
uint8 id2 
int32 position1 
int32 position2 
```

* BulkSetItem 

```
uint8  id1 
uint8  id2 
string item1 
string item2 
int32  value1 
int32  value2 
```  

###dynamixel_sdk_examplesのサービス
dynamixel_sdk_examplesは3つのサービスを持つ 

* GetPosition 

```
uint8 id 
--- 
int32 position  
```
 
* SyncGetPosition 

```
uint8 id1 
uint8 id2 
--- 
int32 position1 
int32 position2 
```

* BulkGetItem 

```
uint8  id1 
uint8  id2 
string item1 
string item2 
--- 
int32  value1 
int32  value2  
```  

## サンプル
``dynamixel_sdk_examples``のサンプルプログラム``read_write_node``を実行してみた．

```
rosrun dynamixel_sdk_examples read_write_node
```


* 注意1
このプログラムは，c++で書かれたもので，そのプログラムコードは``~/catkin_ws/src/DynamixelSDK/ros/dynamixel_sdk_examples/src``の``read_write_node.cpp``に書かれているため，必要であれば，そこを変更して，``catki_ws``で``catkin_make``すれば良い．なお，後に示すが，Pythonのコードを使う場合，変更後保存するだけで適用できる．（インタプリンタ型の特徴）
今回は，このプログラムのデフォルトでのIDが1と2であったが，使っているDynamixelのIDは11~15であるから，とりあえずその2つを11, 12にしておいた．

* 注意2
rosrunで実行するとき，ポートの認識が必要となるため，sudo 666 /dev/ttyUSBによりアクセス権限を与えておく必要がある． 

### 実行結果
以下に実行結果と別のターミナル（端末）で``/set_position``について操作したときの様子を示す．
![set_position.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/3fcf537f-47b6-5f9e-86d4-c321f622d292.png)
![info_position.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/4e8a91c6-90ea-4460-ae70-f7fac30976d4.png)

/set_positionでは，read_write_nodeが購読しておりidとpositionを与えてあげる（配信する）ことで，Dynamixelを動かせるようだ．

## Dynamixelを動かす
先ほどの結果から，とりあえずidとpositionを配信すればモータを駆動できるようだ．今回は試しにコマンドラインから配信してみる．

![pub_position.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/5868e375-8530-9729-b1ce-02ded6e62a7a.png)

配信した後，``read_write_node``では，次のような情報が流れる．
![after_move.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/930a42d8-5041-c095-a41a-e0d29a2b7905.png)

このとき，実際のロボットの様子を以下に示す．
<iframe width="560" height="315" src="https://www.youtube.com/embed/CtNHztMgvHI" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

うまく動かすことができた．Dynamixelモータを動かすときの値については，使用するDynamixelのControlTableを確認する必要がある．

## 感想
やっとDynamixelをROSで動かすことができた．そもそも環境の構築がいまいちわかっていなかったため，ずっと詰まっていた．しかし，そこを乗り越えた途端，色々と理解が深まった．とりあえず，コマンドラインからではあるが，モータ1つの駆動をすることができた．ここまで理解すれば，あとは前回のカメを動かすのと同様であることが分かる．いわば，joyの情報をdynamixelが購読できるようなプログラムを作成してあげれば，コントローラでの制御も可能になると予想できる．次回が楽しみである．

## 参考文献
ROBOTIS e-Manual DYNAMIXEL SDK
(https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) 2021/6/3