はじめに
前回の記事でOpenCR経由でDynamixelをROSノードで動かせるように環境構築を行いました．
今回は公式から配布されているパッケージを使用し，サービス通信でDynamixelを動かしてみます．
.yamlファイルを新規作成
ROSでDynamixelを動かすためには.yamlファイルにDynamixelの情報をrosparamとして読み込ませる必要があります．
早速，動かすためのyamlファイルを作成しましょう．
dynamixel_workbench_controllersパッケージ内のconfigフォルダにmy_config.yamlを新規作成します．
// 長いので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つとも位置制御で動かすように設定します．
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」にしました．
<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してモータを読み込んでみましょう．
$ roslaunch dynamixel_workbench_controllers my_dynamixel_controllers.launch
エラーが発生する場合はbaudrate，IDが合致しているかどうかを確認しましょう．
launchファイル自体が見つからない場合は以下を実行．
$ source ~/catkin_ws/devel/setup.bash
画像のように表示されたら問題なく接続されています．
モータを動かしてみる
指令の送り方を確認してみる
Dynamixelを動かすためにはどのような指令を与えればよいのでしょうか？
下記のコマンドを実行し，どのようなトピック・サービスを受け付けているか確認してみます．
$ rostopic list
$ rosservice list
この中で指令を受け付けているのは
・「/dynamixel_workbench/joint_trajectory」トピック
・「/dynamixel_workbench/dynamixel_command」サービス
の2つです．
したがって，このコントローラでは上記の二通りの方法で動かすことが出来ます．
（上記以外の動かし方，知っている方いましたら教えて下さい...）
「/dynamixel_command」サービスで動かす
今回のきじでは，簡単な動かし方であるサービス通信のやり方について説明します．
使用するサービスファイルは
dynamixel-workbench-msgs/dynamixel_workbench_msgs/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)
試しに以下のコマンドを実行してみましょう．
$ rosservice call /dynamixel_workbench/dynamixel_command '' 0 'Goal_Position' 1000
上記のコマンドは
・commandの名前が''(空欄)
・IDが0
・送信先を'Goal_Position'
・値を1000
にする，といった指令を送っています．
つまり，「IDが0のモータの位置を1000にする」という命令を送っています．
実行してみると，Motor0が回転し，デジタル値で1000の所に対応した角度まで回転します．
IDを変更すれば他のモータも同様に制御できます．
ROSのサービス通信（/dynamixel_command）でdynamixel動かせた pic.twitter.com/SrCAiQBuQH
— F@L (@hayabusa_f) March 31, 2021
ちなみに，速度制御のモータに対して目標速度を命令したい場合は以下のようにします．
$ rosservice call /dynamixel_workbench/dynamixel_command '' 0 'Goal_Velocity' 100
ここで，yamlファイルでモータを
・位置制御モードを選択した場合は位置指令（Goal_Position）のみ
・速度制御モードを選択した場合は速度指令(Goal_Velocity)のみ
しか受け付けないので注意．
負の値を書き込みたい場合
valueとして負のデジタル値をcallしたい場合はトピック・サービス名のあとに「--」をつける．
$ rosservice call /dynamixel_workbench/dynamixel_command -- '' 1 'Goal_Position' -1000
参考：http://wiki.ros.org/ROS/YAMLCommandLine
さいごに
今回の記事でサービス通信を使用して一つのモータを簡単に動かせるようになりました．
しかしながら，
・同時に複数個のモータを動かすことができない
・与えるデータがデジタル値なため変換が面倒
といった問題点もあります．
次回の記事ではもう一つの方法（「/joint_trajectory」トピックでの制御）について解説します．