DynamixelをROSで動かせるようにした話(2) サービス通信で動かしてみる編

# はじめに
[前回の記事](https://qiita.com/FAL19/items/e42b4a86ae026bdb46e7)でOpenCR経由でDynamixelをROSノードで動かせるように環境構築を行いました．
今回は公式から配布されているパッケージを使用し，サービス通信でDynamixelを動かしてみます．


# .yamlファイルを新規作成
**ROSでDynamixelを動かすためには.yamlファイルにDynamixelの情報をrosparamとして読み込ませる必要があります．**

早速，動かすためのyamlファイルを作成しましょう．

dynamixel_workbench_controllersパッケージ内のconfigフォルダにmy_config.yamlを新規作成します．

```bash
// 長いので2行に分ける
$ cd ~/catkin_ws/my_dynamixel_pkgs/dynamixel-workbench
$ cd dynamixel_workbench_controllers/config
$ touch my_config.yaml
```

次にmy_config.yamlファイルを記述します．
それぞれのモータに名前をつけ，モータ番号（ID）や動作モード（Operating Mode）を指定します．
※最低限この2つ（IDとOperating_Mode）は記述するようにしましょう．

（Operating Modeの数字に関しては，以下のリンクを参照．）
https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/#bool-setoperatingmodeuint8_t-id-uint8_t-index-const-char-log--null

今回の例ではモータを3つ使用するのでそれぞれに**Motor0，Motor1，Motor2という名前をつけ，3つとも位置制御**で動かすように設定します．

```yaml:my_config.yaml
Motor0:
  ID: 0
  Return_Delay_Time: 0
  Operating_Mode: 4   # Extended Position Control Mode
  Profile_Acceleration: 0
  Profile_Velocity: 0

Motor1:
  ID: 1
  Return_Delay_Time: 0
  Operating_Mode: 4   # Extended Position Control Mode
  Profile_Acceleration: 0
  Profile_Velocity: 0

Motor2:
  ID: 2
  Return_Delay_Time: 0
  Operating_Mode: 4   # Extended Position Control Mode
  Profile_Acceleration: 0
  Profile_Velocity: 0
```
これをlaunchファイルで読み込ませます．


# launchファイルの編集
次に，dynamixel_workbench_controllers/launch内にあるdynamixel_controllers.launchをコピーして新しくlaunchファイルを作成します．
名前は「my_dynamixel_controllers.launch」にしました．

```xml:my_dynamixel_controllers.launch
<launch>
  <arg name="usb_port"                default="/dev/ttyACM0"/>
  <arg name="dxl_baud_rate"           default="2000000"/>
  <arg name="namespace"               default="dynamixel_workbench"/>

  <arg name="use_moveit"              default="false"/>
  <arg name="use_joint_state"         default="true"/>
  <arg name="use_cmd_vel"             default="false"/>

  <param name="dynamixel_info"          value="$(find dynamixel_workbench_controllers)/config/my_config.yaml"/>

  <node name="$(arg namespace)" pkg="dynamixel_workbench_controllers" type="dynamixel_workbench_controllers"
        required="true" output="screen" args="$(arg usb_port) $(arg dxl_baud_rate)">
    <param name="use_moveit"              value="$(arg use_moveit)"/>
    <param name="use_joint_states_topic"  value="$(arg use_joint_state)"/>
    <param name="use_cmd_vel_topic"       value="$(arg use_cmd_vel)"/>
    <rosparam>
      publish_period: 0.010
      dxl_read_period: 0.010
      dxl_write_period: 0.010
      mobile_robot_config:                <!--this values will be set when 'use_cmd_vel' is true-->
        seperation_between_wheels: 0.160  <!--default value is set by reference of TB3-->
        radius_of_wheel: 0.033            <!--default value is set by reference of TB3-->
    </rosparam>
  </node>
</launch>
```

変更点は
・usb_portを「/dev/ttyACM0」に
・dxl_baud_rateを「Dynamixelに設定したbandrate」に
・dynamixel_infoを先ほど作成した「my_config.yaml」に
の3点です．

また，rosparamの部分の
・publish_period
・dxl_read_period
・dxl_write_period
の数値を小さくすればより高速に通信が行えます．
（今回の環境では全て0.001にしても問題は見受けられませんでした．）

# モータとROSの接続
設定し終わったら，実際にroslaunchしてモータを読み込んでみましょう．

```bash
$ roslaunch dynamixel_workbench_controllers my_dynamixel_controllers.launch
```

エラーが発生する場合はbaudrate，IDが合致しているかどうかを確認しましょう．

launchファイル自体が見つからない場合は以下を実行．

```bash
$ source ~/catkin_ws/devel/setup.bash
```

画像のように表示されたら問題なく接続されています．
![roslaunch_dynamixel.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320997/e9bf2437-b1b6-6fda-b3fb-ead14c3ba0d1.png)



# モータを動かしてみる
### 指令の送り方を確認してみる
Dynamixelを動かすためにはどのような指令を与えればよいのでしょうか？
下記のコマンドを実行し，どのようなトピック・サービスを受け付けているか確認してみます．

```bash
$ rostopic list
$ rosservice list
```
![list.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320997/7312db6b-8916-98eb-2144-6f66081c0282.png)

この中で指令を受け付けているのは
・「/dynamixel_workbench/joint_trajectory」トピック
・「/dynamixel_workbench/dynamixel_command」サービス
の2つです．

したがって，このコントローラでは上記の二通りの方法で動かすことが出来ます．
（上記以外の動かし方，知っている方いましたら教えて下さい...）

# 「/dynamixel_command」サービスで動かす
今回のきじでは，簡単な動かし方であるサービス通信のやり方について説明します．

使用するサービスファイルは
dynamixel-workbench-msgs/dynamixel_workbench_msgs/srv/DynamixelCommand.srv
にあります．

実際に中身を見てみましょう．

```:DynamixelCommand.srv
string command
uint8 id
string addr_name
int32 value
---
bool comm_result
```
requestには
・command（命令の名前．空欄でも問題なし）
・id（動かしたいモータのID）
・addr_name（書き込みたいデータの送信先※1）
・value（書き込むデータ※2）
の4つがあるので，これらを引数として与えrosservice callすればモータが動きそうです．

※1
addr_nameの書き方は，
dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_item.cpp
の一番上の部分を参照．

※2
対応するaddr_name，valueは，それぞれのモータのControl Tableを参照．
(XM540-W270-Rの場合はhttps://emanual.robotis.com/docs/en/dxl/x/xm540-w270/#control-table-of-ram-area)


試しに以下のコマンドを実行してみましょう．

```bash
$ rosservice call /dynamixel_workbench/dynamixel_command '' 0 'Goal_Position' 1000
```

上記のコマンドは
・commandの名前が''(空欄)
・IDが0
・送信先を'Goal_Position'
・値を1000
にする，といった指令を送っています．
つまり，「**IDが0のモータの位置を1000にする**」という命令を送っています．

実行してみると，Motor0が回転し，デジタル値で1000の所に対応した角度まで回転します．
IDを変更すれば他のモータも同様に制御できます．

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">ROSのサービス通信（/dynamixel_command）でdynamixel動かせた <a href="https://t.co/SrCAiQBuQH">pic.twitter.com/SrCAiQBuQH</a></p>&mdash; F@L (@hayabusa_f) <a href="https://twitter.com/hayabusa_f/status/1377237773040558083?ref_src=twsrc%5Etfw">March 31, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>


ちなみに，速度制御のモータに対して目標速度を命令したい場合は以下のようにします．

```bash
$ rosservice call /dynamixel_workbench/dynamixel_command '' 0 'Goal_Velocity' 100
```

ここで，yamlファイルでモータを
・位置制御モードを選択した場合は位置指令（Goal_Position）のみ
・速度制御モードを選択した場合は速度指令(Goal_Velocity)のみ
しか受け付けないので注意．

## 負の値を書き込みたい場合
valueとして負のデジタル値をcallしたい場合はトピック・サービス名のあとに「--」をつける．

```bash
$ rosservice call /dynamixel_workbench/dynamixel_command -- '' 1 'Goal_Position' -1000
```
参考：http://wiki.ros.org/ROS/YAMLCommandLine

# さいごに
今回の記事でサービス通信を使用して一つのモータを簡単に動かせるようになりました．

しかしながら，
・同時に複数個のモータを動かすことができない
・与えるデータがデジタル値なため変換が面倒
といった問題点もあります．

次回の記事ではもう一つの方法（「/joint_trajectory」トピックでの制御）について解説します．