DynamixelHandler-ros2でサーボを動かす

# 初めに

以前の[紹介記事](https://qiita.com/kawawa/items/21752f1a01d78774ac56)で`dynamixel_handler`ノードを使ってコマンドラインからサーボを制御する方法を紹介しました．
この記事では，プログラムから，すなわち制御用の別ノードからトピックを Pub & Sub してサーボを動かす[example](https://github.com/ROBOTIS-JAPAN-GIT/DynamixelHandler-ros2/tree/main/dynamixel_handler_examples)を動かしてみます．
コードの詳細についての解説は [examplesパッケージのReadMe](https://github.com/ROBOTIS-JAPAN-GIT/DynamixelHandler-ros2/tree/main/dynamixel_handler_examples#readme) を参照のこと．
また，`dynamixel_handler` pkg と連携するための pkg 自体の作り方についての解説は別記事で書きます．

# 0. パッケージの導入

必要なパッケージは `dynamixel_handler` と `dynamixel_handler_examples` です．
どちらも[こちらのリポジトリ](https://github.com/ROBOTIS-JAPAN-GIT/DynamixelHandler-ros2)に入っています．
導入については以前の[紹介記事](https://qiita.com/kawawa/items/21752f1a01d78774ac56#how-to-install)を参考にしてください．

# 1. Dynamixelの接続

Dynamixel Wizardでモータの動作確認ができる程度の状態を想定しています．
Dynamixel Wizardってなに？という人は公式のチュートリアルをやりましょう．

- DynaimixelをディジーチェーンにしてU2D2経由でUSB接続されていること．
- idに重複がないように事前に設定されていること.
- baudrateが全て統一されていること．

`dynamixel_handler` pkg のReadmeに書いてありますが，ver0.2.2から，Dynamixelのbaudrateを自動で設定する機能が追加されました．baudrateの統一が面倒な人は`init/baudrate_auto_set`を`true`に設定したうえで`dynamixel_handler`を起動してみてください．

# 2. `dynamixel_handler` ノードの起動

## 2-1. ビルド

念のため適当なターミナルで以下のコマンドを実行し，再ビルドしておきましょう．
```bash
cd ~/ros2_ws
colcon build --symlink-install --packages-up-to dynamixel_handler
source ~/ros2_ws/install/setup.bash # 初回 build 時のみ
```

## 2-2. 自分の環境とconfigの設定を合わせる
`dynamixel_handler/config/config_dynamixel_handler.yml`の該当部分を編集し，保存．   
以下は baudrate: $57600$ かつ device name: `/dec/ttyUSB0` かつ latency timer: $16$ ms の場合
```yml
# dynamixel_handler/config/config_dynamixel_handler.yaml
/**:
    ros__parameters:
        # 通信機器の設定
        device_name: /dev/ttyUSB0 # 通信するデバイス名,
        baudrate: 57600    # 通信速度, Dynamixelのデフォルトは 57600 bps (で遅い) 
        latency_timer: 16 # 通信のインターバル, 多くの環境でデフォルトは 16 ms (で遅い)
```

## 2-3. ターミナルから実行

```bash
ros2 launch dynamixel_handler dynamixel_handler_launch.xml
```
連結したDynamixelが自動で探索され，見つかったDynamixelの初期設定(トルクオン，Profileの設定など)が行われる．   
うまく見つからない場合は (1) Baudrate, (2)Device name, (3)Device の実行権限, (4)Latency timer 当たりの設定が間違っていることが多い．
(3)について，一時的な変更であれば `$ sudo chmod 666 /dev/ttyUSB0` を実行すればいい．(`/dev/ttyUSB0`は自分の環境の`device_name`に合わせて変えてください)

# 3. `example1` ノードを起動

## 3-1. ビルド

新しいターミナルで以下のコマンドを実行し `dynamixel_handler_examples` パッケージをビルド．
```bash
cd ~/ros2_ws
colcon build --symlink-install --packages-up-to dynamixel_handler_examples
source ~/ros2_ws/install/setup.bash # 初回 build 時のみ
```

## 3-2. ターミナルから実行

以下のコマンドを`dynamixel_handler`ノードとは異なるターミナルで以下を実行．
```bash
ros2 launch dynamixel_handler_examples example1.xml
```

# 4. うまくいった場合の出力例

Dynamixelの接続，`dynamixel_handler` ノードの起動，`example1` ノードの起動がうまくいった場合は次のような出力が得られるはずです．

#### Dynamixel 実機
全てのサーボが1秒周期で+-45degの往復運動．
型番によるが，サーボホーンを手で触ると往復を止められるくらい弱いトルクで動作しているはず．

#### `dynamixel_handler` ノードを起動してるターミナルの出力
以下のようにtopicをCallbackしたことがlogとして流れているはず．
```
# 出力例, ID: 1,2,6,7のサーボがつながっている場合
[dynamixel_handler_node-1] 1735733942.115433173: Loop [1800]: total=10.72ms(read=18.47ms), success=100%(full=100%) 
[dynamixel_handler_node-1] 1735733942.195878332: ===================================== 
[dynamixel_handler_node-1] 1735733942.195931332: Status cmd, '4' servo(s) are tryed to updated
[dynamixel_handler_node-1] 1735733942.195947172:   ID: [ 1, 2, 6, 7 ]
[dynamixel_handler_node-1] 1735733942.195950350:   - change torque mode
[dynamixel_handler_node-1] 1735733942.195955451: ==================+================== 
[dynamixel_handler_node-1] 1735733942.195960229: Current-base Position ctrl(X), ID: [ 1, 2, 6, 7 ]
[dynamixel_handler_node-1] 1735733942.195969358: Goal cmd '4' servo(s) are tried to update
[dynamixel_handler_node-1] 1735733942.195974298:   ID: [ 1, 2, 6, 7 ]
[dynamixel_handler_node-1] 1735733942.195976553:   - updated: goal current
[dynamixel_handler_node-1] 1735733942.195978922:   - updated: goal position
[dynamixel_handler_node-1] 1735733942.195986110: ============+===========+============
```
もし，実機が動作しているのにlogが流れていなかったら，configファイルでlog出力しないようにしている可能性が高い．(出さなくても特に問題はない．)

#### `example1` ノードを起動してるターミナルの出力
以下のように，Subscribe したトピックからサーボの状態と現在値が出力されいているはず．ただのサンプルなので特に見やすくはない．
```
# 出力例
[example1-1] 1735733943.189341739: * Present value updated time 1735733943.165528 
[example1-1] 1735733943.189398796: * servo [1], pos -45.000000, vel 0.000000, cur 300.000000
[example1-1] 1735733943.189405258: * servo [2], pos 39.814500, vel 98.928000, cur -112.980000
[example1-1] 1735733943.189409155: * servo [6], pos -45.000000, vel 0.000000, cur 300.000000
[example1-1] 1735733943.189412573: * servo [7], pos -45.000000, vel 0.000000, cur 300.000000
[example1-1] 1735733943.283756737: - servo [1], torque on, has no error, ping is response, mode is cur_position 
[example1-1] 1735733943.283800802: - servo [2], torque on, has no error, ping is response, mode is cur_position 
[example1-1] 1735733943.283804613: - servo [6], torque on, has no error, ping is response, mode is cur_position
[example1-1] 1735733943.283806784: - servo [7], torque on, has no error, ping is response, mode is cur_position
[example1-1] 1735733943.987708336: - servo [1], torque on, has no error, ping is response, mode is cur_position 
[example1-1] 1735733943.987759457: - servo [2], torque on, has no error, ping is response, mode is cur_position
[example1-1] 1735733943.987762907: - servo [6], torque on, has no error, ping is response, mode is cur_position
[example1-1] 1735733943.987765015: - servo [7], torque on, has no error, ping is response, mode is cur_position
[example1-1] 1735733944.189360819: * Present value updated time 1735733944.155483 
[example1-1] 1735733944.189415396: * servo [1], pos 45.000000, vel 0.000000, cur 300.000000 
[example1-1] 1735733944.189421887: * servo [2], pos -39.990200, vel -97.554000, cur -8.070000
[example1-1] 1735733944.189425889: * servo [6], pos 45.000000, vel 0.000000, cur 300.000000
[example1-1] 1735733944.189436333: * servo [7], pos 45.000000, vel 0.000000, cur 300.000000
```
現在値の出力については1Hzになっているはずなので，飛び飛びの値が読み取れるはず．
リアルタイムな動作を確認したい場合は，下記のように`/dynamixel/debug` トピックを`$ ros2 topic echo`で確認するのが良い．

#### `/dynamixel/debug` トピックの確認
別のターミナルを開き，以下のコマンドを実行することでサーボの現在値や状態などをリアルタイムで確認できる．
```bash
ros2 topic echo --flow-style /dynamixel/debug
```
```yml
# 出力例
---
status:
  id_list: [1, 2, 6, 7]
  torque: [true, true, true, true]
  error: [false, false, false, false]
  ping: [true, true, true, true]
  mode: [cur_position, cur_position, cur_position, cur_position]
current_ma:
  present: [0.0, 0.0, 0.0, 0.0]
  goal: [300.0, 301.28, 300.0, 300.0]
velocity_deg_s:
  present: [0.0, 0.0, 0.0, 0.0]
  goal: [199.23, 199.23, 199.23, 199.23]
position_deg:
  present: [-45.0, 45.0, -45.0, -45.0]
  goal: [-45.0, 45.0, -45.0, -45.0]
---
```

# 終わりに

今回の記事では，`dynamixel_handler` および `dynamixel_handler_examples` を用いて，ROS 2 環境下でサーボを制御する基本的な流れをご紹介しました．セットアップ，各種設定ファイルの編集，ノードのビルド・実行，さらにはデバッグ情報の確認方法を通して，実際のシステムへの組み込みイメージを掴んでいただければ幸いです．

今後は，`dynamixel_handler` と連携するための独自パッケージの作り方についても解説していく予定です.