初めに
以前の紹介記事でdynamixel_handlerノードを使ってコマンドラインからサーボを制御する方法を紹介しました．
この記事では，プログラムから，すなわち制御用の別ノードからトピックを Pub & Sub してサーボを動かすexampleを動かしてみます．
コードの詳細についての解説は examplesパッケージのReadMe を参照のこと．
また，dynamixel_handler pkg と連携するための pkg 自体の作り方についての解説は別記事で書きます．
0. パッケージの導入
必要なパッケージは dynamixel_handler と dynamixel_handler_examples です．
どちらもこちらのリポジトリに入っています．
導入については以前の紹介記事を参考にしてください．
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. ビルド
念のため適当なターミナルで以下のコマンドを実行し，再ビルドしておきましょう．
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 の場合
# 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. ターミナルから実行
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 パッケージをビルド．
cd ~/ros2_ws
colcon build --symlink-install --packages-up-to dynamixel_handler_examples
source ~/ros2_ws/install/setup.bash # 初回 build 時のみ
3-2. ターミナルから実行
以下のコマンドをdynamixel_handlerノードとは異なるターミナルで以下を実行．
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 トピックの確認
別のターミナルを開き，以下のコマンドを実行することでサーボの現在値や状態などをリアルタイムで確認できる．
ros2 topic echo --flow-style /dynamixel/debug
# 出力例
---
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 と連携するための独自パッケージの作り方についても解説していく予定です.