Joy-ConでROS2の亀を動かす（ROS1からROS2へ移植）

# joycon_controller
こちらの[GitHub](https://github.com/Azicode/joycon_controller)に手順とソースをまとめてあります。 
手早く動かしたい方は上のリンクを参照してください。
![VID_20220105_vertical_Trim.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/556756/d751e586-c811-98d6-8741-dbe791133f54.gif)

## 本記事の内容
先に本題を書いておきます。 
Joy-ConをROS2で使えるようにするにあたって、

- [1. ROS1からROS2へのPythonスクリプトの書き換え](#1-ros1からros2へのpythonスクリプトの書き換え)
    - [1.1. rospyとrclpyの書き換え](#11-rospyとrclpyの書き換え)
    - [1.2. Python2とPython3の書き換え](#12-python2とpython3の書き換え)
- [2. 実際に動かしてみる](#2-実際に動かしてみる)
    - [2.1. インストールと起動](#21-インストールと起動)

についてやったことをまとめていきます。今回に関してはROS2の書き換えの話しか書いておりません。コードはGitHubにあるので参照したい方は[こちら](https://github.com/Azicode/joycon_controller)からお願いします。
pygameやコントローラー関係のスクリプトについては調べきれてないので後から更新するかも。 

## 実行環境
CPU: intel core i5 8250U
OS : Ubuntu 20.04.3LTS
ROS: ROS2 foxy

## 0. はじめに
お正月休みということで、発売から5年に近づいたNintendo Switchのコントローラー、いわゆるJoy-Conを使ってROS2のコントローラーにしてしまおう！という目論見のもと、ROS1用に書かれていた[こちらの記事](https://qiita.com/koichi_baseball/items/59e093211e9f6be9482f)をROS2用に手を加えてみました。  
今回はライブラリに[pygame](https://www.pygame.org/docs/)を採用していて、傾き検知などの凝った技術は使えていませんが、それでもJoy-Conは片手で操作しやすくていいなあと感じております。
ロボット用の無線のコントローラーに困っている方は、JoyConという選択肢を考えてみてはいかがでしょうか。

##1. ROS1からROS2へのPythonスクリプトの書き換え
PythonのROS1からROS2への書き換え
基本的にはrospyの部分をrclpyのNodeを使って書き直してあるだけです。
ROS2の基本的なコードの説明は次の金沢工業大学の記事が大変参考になりました。
[ロボットプログラミングⅡ-2021：ROS2演習5-サービス通信しよう！(Python)](https://demura.net/education/lecture/21725.html)
コードのコメントがとても丁寧に書いてあって、初心者の方は理解が捗るはずです。

###1.1. rospyとrclpyの書き換え
#### Initialize Node
コンストラクタの最初の行に書く部分です。

```Python
ROS1:	rospy.init_node('joycon_node', anonymous=True)
ROS2:	super().__init__('joycon_node')
```
書き方は違いますがやっていることは同じで、サービス名を設定しています。


#### Create Publisher
```Python
ROS1:	self.twist_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=1000)
ROS2:	self.twist_pub = self.create_publisher(Twist, '/turtle1/cmd_vel', 1000)
```
引数の順番が変わっているのに注意してください。
ROS2の方で見ると、第1引数に型名、第2引数にトピック名、第3引数にはキューサイズといいQoS通信に利用されます。 
型名とトピック名のところをSubscriberと合わせなければ通信できないので注意しましょう。

#### Create Timer
タイマーを設定して周期的にコールバック関数を呼びだす部分です。

```Python
ROS1:	rospy.Timer(rospy.Duration(0.1), self.timerCallback)
ROS2:	timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)
```
これも、使い方は共通していて、第1引数に周期、第2引数にコールバック関数を設定します。 
ROS2の方ではself.timerというクラス変数に格納しているようですが、これを後で使っているのは見かけませんでした。ほおっておいても問題なさそうです。

#### Main

```Python
ROS1: if __name__ == '__main__':
          try:
              jc = joyconController()
              rospy.spin()
          except pygame.error:
              print 'joystickが見つかりませんでした。'
          except rospy.ROSInterruptException: pass

ROS2: def ros_main(args=None):
          rclpy.init(args=args)
          
          joycon_controller_class = joycon_controller()
          rclpy.spin(joycon_controller_class)

          joycon_controller.destroy_node()
          rclpy.shutdown()

      if __name__ == '__main__':
          ros_main()
```
かなり書き方が違いますが、見本通りに書き換えればいいと思います。


###1.2. Python2とPython3の書き換え

ROS1はPython2、ROS2はPython3なので書き換える必要がある部分があります。
他にもあると思われますが、今回はprintに()を付けるだけで済みました。

```Python
Python2: print 値
Python3: print(値)
```
Python2ではprint分、Python3ではprint関数という違いがあるので、このような書き換えが必要です。

##2. 実際に動かしてみる
###2.1 インストールと起動
#### turtlesimを起動する
チュートリアルでおなじみの亀ちゃんを起動していきます。
端末を起動して以下のコマンドを入力してください。

```shell
# turtlesimパッケージをインストールする
$ sudo apt update
$ sudo apt install ros-foxy-turtlesim
# ノードを起動する
$ ros2 run turtlesim turtlesim_node
```

#### joycon_controllerをインストールする
pipが必要なのでインストールされていない場合は先にpipをインストールしてください。

```shell
# pip3のVersionを確認
$ pip3 -V
# pip3をインストールする
$ sudo apt install python3-pip
# pip3をアップグレードする
$ pip3 install pip --upgrade
```
joycon_controllerのパッケージとpygameをインストールしていきます。
ワークスペース名は好きに決めてください。

```shell
$ . /opt/ros/foxy/setup.bash
$ cd
$ mkdir -p ros2_ws/src
$ cd ros2_ws/src

$ git clone https://github.com/Azicode/joycon_controller.git
$ pip3 install pygame

$ cd ~/ros2_ws
$ rosdep install -i --from-path src --rosdistro foxy -y
$ colcon build --symlink-install
```

#### joycon_controllerを起動する。
ビルドが成功したら新しい端末を開いてください。
先にJoy-ConをPCに繋いでおかないと起動に失敗するので、Bluetoothから繋いでおいてください。
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/556756/26c308ee-6b93-1e8b-5bd9-e803575f4fff.jpeg" width="300">

```shell
$ cd ros2_ws
$ . /opt/ros/foxy/setup.bash
$ . install/setup.bash

# 起動！！
$ ros2 run joycon_controller joycon_controller
```

###2.2. 向きを変える
[実行ファイル](https://github.com/Azicode/joycon_controller/blob/master/joycon_controller/joycon_controller.py)の16行目、ROTATIONの値を変更すればJoy-Conを横持にしたり左スティックに対応することができます。ROTATIONの値を2にして、横持にしてみました。

```Python3
#-------- Rotation --------#
# JoyCon R, Vertical   => 0
# JoyCon L, Vertical   => 1
# JoyCon R, Horizontal => 2
# JoyCon L, Horizontal => 3
ROTATION = 2
```
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/556756/2f8732b0-fffe-9072-9bbe-aa3a9ef466f7.gif" width="300">

質問・指摘などあればコメントお願いします。
おしまい。

###参考リンク
- [ROS2公式PythonでPublisherを作る](https://docs.ros.org/en/foxy/Tutorials/Writing-A-Simple-Py-Publisher-And-Subscriber.html)
- [金沢工業大学 deruma.net](https://demura.net/education/lecture)
- [Nintendo Switchのジョイコン(L)をROSのコントローラとして使う](https://qiita.com/koichi_baseball/items/59e093211e9f6be9482f)