Use DYNAMIXEL motor in ROS1

#  ROS1でDynamixelのモータを動かす。
- OS環境 :Ubuntu 20.04
- ROS環境:ROS1 Noetic
#### 目次
[環境構築](#環境構築)
[動作確認](#動作確認)

[参考元](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/)
[エラーが出た場合はこちら](https://github.com/ROBOTIS-GIT/dynamixel-workbench/issues)

# 環境構築
### 1.ワークスペスの作成
```
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin_make
cd
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
```

### 2.ライブラリをダウンロードする。
```
cd ~/catkin_ws/src
git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench.git
git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git
git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git
```
### 3.DYNAMIXEL SDKライブラリをセットアップする。
```
cd ~/catkin_ws/src/DynamixelSDK/c++/build/linux64
make
```
実行結果は以下のようになった。エラーが見られるが、現時点では問題なく動作している。
```
user@PC:~/catkin_ws/src/DynamixelSDK/c++/build/linux64$ make
mkdir -p ./.objects/
g++ -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c -I../../include/dynamixel_sdk -m64 -fPIC -g -c ../../src/dynamixel_sdk/group_bulk_read.cpp -o .objects/group_bulk_read.o
../../src/dynamixel_sdk/group_bulk_read.cpp: In member function ‘bool dynamixel::GroupBulkRead::getError(uint8_t, uint8_t*)’:
../../src/dynamixel_sdk/group_bulk_read.cpp:235:19: warning: suggest parentheses around assignment used as truth value [-Wparentheses]
  235 |   return error[0] = error_list_[id][0];
      |          ~~~~~~~~~^~~~~~~~~~~~~~~~~~~~
g++ -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c -I../../include/dynamixel_sdk -m64 -fPIC -g -c ../../src/dynamixel_sdk/group_bulk_write.cpp -o .objects/group_bulk_write.o
g++ -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c -I../../include/dynamixel_sdk -m64 -fPIC -g -c ../../src/dynamixel_sdk/group_sync_read.cpp -o .objects/group_sync_read.o
../../src/dynamixel_sdk/group_sync_read.cpp: In member function ‘bool dynamixel::GroupSyncRead::getError(uint8_t, uint8_t*)’:
../../src/dynamixel_sdk/group_sync_read.cpp:204:19: warning: suggest parentheses around assignment used as truth value [-Wparentheses]
  204 |   return error[0] = error_list_[id][0];
      |          ~~~~~~~~~^~~~~~~~~~~~~~~~~~~~
g++ -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c -I../../include/dynamixel_sdk -m64 -fPIC -g -c ../../src/dynamixel_sdk/group_sync_write.cpp -o .objects/group_sync_write.o
g++ -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c -I../../include/dynamixel_sdk -m64 -fPIC -g -c ../../src/dynamixel_sdk/packet_handler.cpp -o .objects/packet_handler.o
g++ -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c -I../../include/dynamixel_sdk -m64 -fPIC -g -c ../../src/dynamixel_sdk/port_handler.cpp -o .objects/port_handler.o
g++ -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c -I../../include/dynamixel_sdk -m64 -fPIC -g -c ../../src/dynamixel_sdk/protocol1_packet_handler.cpp -o .objects/protocol1_packet_handler.o
g++ -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c -I../../include/dynamixel_sdk -m64 -fPIC -g -c ../../src/dynamixel_sdk/protocol2_packet_handler.cpp -o .objects/protocol2_packet_handler.o
g++ -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c -I../../include/dynamixel_sdk -m64 -fPIC -g -c ../../src/dynamixel_sdk/port_handler_linux.cpp -o .objects/port_handler_linux.o
g++ -shared -fPIC -m64 -o ./libdxl_x64_cpp.so ./.objects/group_bulk_read.o ./.objects/group_bulk_write.o ./.objects/group_sync_read.o ./.objects/group_sync_write.o ./.objects/packet_handler.o ./.objects/port_handler.o ./.objects/protocol1_packet_handler.o ./.objects/protocol2_packet_handler.o ./.objects/port_handler_linux.o -lrt

```
### 4.DYNAMIXEL Workbenchライブラリをセットアップする。
```
cd ~/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_toolbox/examples
mkdir -p build && cd build
cmake ..
make
```
以下のようなエラーが出たとしても後のlaunchファイルが実行できる。
```
user@user:~/dyna_ws/src/my_dyna_pkgs/dynamixel-workbench/dynamixel_workbench_toolbox/examples/build$ make
[  2%] Building CXX object CMakeFiles/dynamixel_workbench.dir/home/rentaro/dyna_ws/src/my_dyna_pkgs/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp.o
In file included from /home/rentaro/dyna_ws/src/my_dyna_pkgs/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp:19:
/home/rentaro/dyna_ws/src/my_dyna_pkgs/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/../../include/dynamixel_workbench_toolbox/dynamixel_driver.h:29:12: fatal error: dynamixel_sdk/dynamixel_sdk.h: No such file or directory
   29 |   #include "dynamixel_sdk/dynamixel_sdk.h"
      |            ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [CMakeFiles/dynamixel_workbench.dir/build.make:76: CMakeFiles/dynamixel_workbench.dir/home/rentaro/dyna_ws/src/my_dyna_pkgs/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:162: CMakeFiles/dynamixel_workbench.dir/all] Error 2
make: *** [Makefile:84: all] Error 2
```

### 5.デバイス(U2D2)をセットアップする。
```
cd
wget https://raw.githubusercontent.com/ROBOTIS-GIT/dynamixel-workbench/master/99-dynamixel-workbench-cdc.rules
sudo cp ./99-dynamixel-workbench-cdc.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules
sudo udevadm trigger
```

# 動作確認
| 準備物 |
|:-----------------|
| DYNAMIXEL Motor RX-24F | 
| U2D2 |
| DXSharingBoard |
| Robot Cable-X4P(JST Cable) |
| Robot Cable-X4P(Molex-JST Convert Cable) |
| MicroB to A Cable |
| Power Supply |

[機器のセットアップ方法はこちら](https://emanual.robotis.com/docs/en/parts/interface/u2d2/)

### 1.USBポートを確認する。
モータとPCをUSB接続する前に以下のコマンドを実行し、モータとPCをUSB接続した後に以下のコマンドを実行する。
前後を比較して増えたポートがUSBポートの名前である。
```
ls /dev/tty*
```
### 2.DYNAMIXELモータの接続を確認する。
- それぞれ別ターミナルで実行する。
```
roscore
```
- USBポートがttyUSB0だった場合のコマンド例。
```
rosrun dynamixel_workbench_controllers find_dynamixel /dev/ttyUSB0
```
結果例
```
user@pc:~$ rosrun dynamixel_workbench_controllers find_dynamixel /dev/ttyUSB0
[ INFO] [1672040799.571003351]: Succeed to init(9600)
[ INFO] [1672040799.571034889]: Wait for scanning...
[ INFO] [1672040821.361437230]: Find 0 Dynamixels
[ INFO] [1672040821.362668570]: Succeed to init(57600)
[ INFO] [1672040821.362706772]: Wait for scanning...
[ INFO] [1672040839.373363219]: Find 1 Dynamixels
[ INFO] [1672040839.373434504]: id : 1, model name : RX-24F
[ INFO] [1672040839.389765387]: Succeed to init(115200)
[ INFO] [1672040839.389814475]: Wait for scanning...
[ INFO] [1672040857.055237444]: Find 0 Dynamixels
[ INFO] [1672040857.056676213]: Succeed to init(1000000)
[ INFO] [1672040857.056725217]: Wait for scanning...
^C

```
接続確認とともに、使用するモータが割り当てられているIDを確認する。
新たにIDを割り当てる場合[DYNAMIXEL Wizard 2.0](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/)をPCにインストールし、そのソフトウェアの中で割り当てる。

- もし以下のようなエラーが出た場合、
```
[rosrun] Couldn't find executable named find_dynamixel below /home/user/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_controllers
```
改めてビルドする。
```
cd ~/catkin_ws
catkin_make
```
### 3.モータを動かす。
- 現在のモータの角度を知りたい場合
rosbagコマンドを使用すると現在のモータの角度を知ることができる。rosbagとはトピックデータを記録するときに便利なツールである。また、これを使ってモータの角度変化をグラフ化することもできる。
すべてのトピックを記録するときのコマンド
```
roscore
rosbag record -a -O bagfilename
```
記録したトピックを可視化するときのコマンド
```
roscore
rqt_bag bagfilename.bag
```
ウィンドウが立ち上がったら左に並んだトピック名dynamixel_workbench/joint_statesの上で右クリックし、view > plotから右に出たsensor_msgs/JointState内のPositionにチェックを入れることでグラフを出すことができる。

[詳細はこちら 1](https://qiita.com/srs/items/f6e2c36996e34bcc4d73)
[詳細はこちら 2](https://qiita.com/RENOX/items/5b9185cf225968988424)

- 動かすモータが2つの場合
デフォルトでは2つのモータを動かす設定になっているので、何も触らずに以下を実行する。
```
roslaunch dynamixel_workbench_controllers dynamixel_controllers.launch
```
RX-24Fモータを位置制御するコマンド例。
command:今回は空欄
id:1(割り当てたID)
addr_name:書き込みたいデータ名([RX-24Fコントロールテーブル](https://emanual.robotis.com/docs/en/dxl/rx/rx-24f/)より)
value:書き込むデータ値([RX-24Fコントロールテーブル](https://emanual.robotis.com/docs/en/dxl/rx/rx-24f/)よりCW Angle LimitとCCW Angle Limitにある0~1023の値を設定する。)
```
rosservice call /dynamixel_workbench/dynamixel_command '' 1 'Goal_Position' 1023
```

- 1つのモータを動かす場合
以下のようにyamlファイルを修正する必要がある。

まず、launchファイルを確認する。
```
cd ~/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_controllers/launch
gedit dynamixel_controllers.launch
```
```
<launch>
  <arg name="usb_port"                default="/dev/ttyUSB0"/>
  <arg name="dxl_baud_rate"           default="57600"/>
  <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/basic.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>
```
launchファイルを見ると設定としてbasic.yamlが読み込まれていることがわかるので、basic.yamlを以下のように修正する。
```
cd ~/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_controllers/config
gedit basic.yaml
```
ID:2が割り当てられたtiltという名前のモータをコメントアウトする。これは使用するモータがID:1に割り当てられていることを前提にしている。
```
# You can find control table of Dynamixel on emanual (http://emanual.robotis.com/#control-table)
# Control table item has to be set Camel_Case and not included whitespace
# You are supposed to set at least Dynamixel ID
 pan:
   ID: 1
   Return_Delay_Time: 0
#tilt:
#  ID: 2
#  Return_Delay_Time: 0
```
そして、以下のコマンドをターミナルで実行する。
```
roslaunch dynamixel_workbench_controllers dynamixel_controllers.launch
```
別のターミナルで以下のコマンドを実行するとモータが回転する。
```
rosservice call /dynamixel_workbench/dynamixel_command '' 1 'Goal_Position' 1023
```
- 逆回転したい場合は負の値を実行する。
```
rosservice call /dynamixel_workbench/dynamixel_command -- '' 1 'Goal_Position' -1023
```
# Dynamixelモータに関する記事集
[Dynamixel Workbenchを触ってみた #01](https://dream-drive.net/2019/07/14/9402/)
[Dynamixel Workbenchを触ってみた #02](https://dream-drive.net/2019/07/15/9446/)
[Dynamixel Workbenchを触ってみた #03](https://dream-drive.net/2019/07/17/9468/)
[Dynamixel Workbenchを触ってみた #04](https://dream-drive.net/2019/07/18/9521/)
[Dynamixel Workbenchを触ってみた #05](https://dream-drive.net/2020/01/18/9695/)
[ROSの勉強　第43弾：Dynamixel](https://qiita.com/Yuya-Shimizu/items/04d73333bf0a870be13f)

# まとめ
今回、DYNAMIXELのモータを使えるように環境構築を行った。
動作確認ではコントローラー機能を使ったが、他にもオペレーター機能、ホイールオペレーター機能があるので、コントローラー機能を参考にlaunchファイルやyamlファイルを確認して使ってみると良い。

---
<font color="yellowgreen">**QiitaLink**</font>
<font color="black">**Designed by RENOX**</font>