はじめに

ROS2のプログラミングの勉強（基礎編）とし実施したことをまとめて残していきます．
このページではTurtlebot3をゲームパッド操作で動かすためのプログラムを自作します．

環境

・PC: Panasonic Let's Note CF-FV
・OS: Windows11 Pro (64bit)
・VirtualBox: 6.1.38 (Ubuntu20.04.5, GUI環境)
・ROS: ROS2 Foxy

・ロボット：Turtlebot3 burger (Raspberry Pi 3B+)

・ゲームパッド：Logicool F710

ここまでの環境構築については下記参照
・Windows PCにVirtualBox＋Ubuntuを導入
https://qiita.com/pez/items/a3ef1855f7e1e0ed3dfd
・ROS(ROS2 Foxy)をインストール
https://qiita.com/pez/items/1df36628524ff40a3d93
・Turtlebot3 burger のセットアップ
https://qiita.com/pez/items/1d3d15b3911d5dab1702

1. ゲームパッド接続・設定

ここではゲームパッドとしてLogicoolのF710を使いますが，USBで接続するタイプであれば他の製品でもほぼ同じようにできると思われます．
まずは下記別記事の「Ubuntuの設定」までを行います
（ROS1に関する手順ですがそこまでは同じです）
https://qiita.com/pez/items/cda66c1225c44ba975dd

pygameのインストール

$ pip3 install pygame

※pip3が未インストールの場合は
「sudo apt install python3-pip」で入れる

次にROS2とは関係なく，pythonプログラムでゲームパッドの入力を確認してみましょう．場所はどこでもよいので下記のようなファイル（拡張子は.py）を作成します．（ここでは例としてgamepadtest.py）

gamepadtest.py

import pygame
from pygame.locals import *

# ジョイスティックの初期化
pygame.joystick.init()
try:
   # ジョイスティックインスタンスの生成
   joystick = pygame.joystick.Joystick(0)
   joystick.init()
   print('ジョイスティックの名前:', joystick.get_name())
   print('ボタン数 :', joystick.get_numbuttons())
except pygame.error:
   print('ジョイスティックが接続されていません')

# pygameの初期化
pygame.init()

# ループ
active = True
while active:
   # イベントの取得
   for e in pygame.event.get():
       # 終了ボタン
       if e.type == QUIT:
           active = False

       # ジョイスティックのボタンの入力
       if e.type == pygame.locals.JOYAXISMOTION:
           print('左ジョイスティック(0)(1):', joystick.get_axis(0), joystick.get_axis(1))
           print('右ジョイスティック(3)(4):', joystick.get_axis(3), joystick.get_axis(4))
           print('LトリガーRトリガー(2)(5):', joystick.get_axis(2), joystick.get_axis(5))
           
       elif e.type == pygame.locals.JOYBUTTONDOWN:
           print('ボタン'+str(e.button)+'を押した')
       elif e.type == pygame.locals.JOYBUTTONUP:
           print('ボタン'+str(e.button)+'を離した')

できたら下記のように実行します．

$ python3 gamepadtest.py

ゲームパッドを操作してそれに応じた表示が出れば成功です．十字キーは左ジョイスティックと「modeボタン」で切り替わるようになっています．Ctrl＋c で終了．

ある程度Pythonを勉強した人ならあとはTeleopの自作
https://qiita.com/pez/items/fbb58887501f5cb9ccee
を参考に自分で作ってしまえるのでチャレンジしてみましょう．
以下は一つの例として参考にして下さい．

2. ワークスペース作成

今回は前回使った teleop_ws を再利用することにします．もちろん新しいワークスペースを作成してもOKです．

$ cd ~/teleop_ws/src/

3. パッケージ作成

今回ノード名は my_gamepad_node
パッケージ名は my_gamepad
にすることにします

$ cd ~/teleop_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_gamepad_node my_gamepad

4. プログラミング

4.1 package.xml ファイルの編集

$ cd ~/teleop_ws/src/my_gamepad
$ emacs package.xml

自作パッケージを一般公開するときには変更する必要がありますが，今回も特になにもしません．

4.2 setup.py ファイルの編集

$ cd ~/teleop_ws/src/my_gamepad
$ emacs setup.py

23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要があります．今回もパッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっているはずです．

entry_point={
    'console_scripts': [
        'my_gamepad_node = my_gamepad.my_gamepad_node:main' 
    ],
},

※もし１つのパッケージに複数のPythonファイルがある場合はそのファイルごとにエントリポイントを指定しなければならないので setup.py を変更する必要があります．

4.3 ソースコード作成

今回もパッケージ作成時に自動的に作られている my_gamepad_node.py を編集することにします．

$ cd ~/teleop_ws/src/my_gamepad/my_gamepad
$ emacs my_gamepad_node.py

下記のように記述してみましょう．

my_gamepad_node.py

import sys
import rclpy                        
from rclpy.node import Node          
from geometry_msgs.msg import Twist  # Twist メッセージ型をインポート
import pygame
from pygame.locals import *


class MyGamepad(Node):  # ゲームパッド操作により速度指令値をパブリッシュするクラス
    def __init__(self):   # コンストラクタ
        super().__init__('my_gamepad_node')        
        self.publisher = self.create_publisher(Twist, 'cmd_vel', 10)
        self.timer = self.create_timer(0.01, self.timer_callback)
        self.vel = Twist()  # Twist メッセージ型インスタンスの生成
        self.vel.linear.x = 0.0
        self.vel.angular.z = 0.0

        # ジョイスティックの初期化
        pygame.joystick.init()
        try:
            # ジョイスティックインスタンスの生成
            self.joystick = pygame.joystick.Joystick(0)
            self.joystick.init()
            print('ジョイスティックの名前:', self.joystick.get_name())
            print('ボタン数 :', self.joystick.get_numbuttons())
        except pygame.error:
            print('ジョイスティックが接続されていません')

        # pygameの初期化
        pygame.init()


    def timer_callback(self):  # タイマーのコールバック関数
        for e in pygame.event.get():
            linearspeed = 0.0
            rotation = 0.0
            if e.type == QUIT:
                break
            
            if e.type == pygame.locals.JOYAXISMOTION:
                linearspeed = - float(self.joystick.get_axis(1)) * 0.2
                rotation = - float(self.joystick.get_axis(0)) * 0.8

            self.vel.linear.x = linearspeed
            self.vel.angular.z = rotation
        
            self.publisher.publish(self.vel)  # 速度指令メッセージのパブリッシュ
            self.get_logger().info(f'並進速度={self.vel.linear.x} 角速度={self.vel.angular.z}')


def main():  # main関数
    rclpy.init()
    node = MyGamepad()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print('Ctrl+Cが押されました．')
    except ExternalShutdownException:
        sys.exit(1)
    finally:
        rclpy.try_shutdown()

5. ビルド

ワークスペース名直下のディレクトリ以外はビルドできません
初めてビルドしたとき build, install, log ディレクトリが作成されます

$ cd ~/teleop_ws
$ colcon build

成功すると小さな別ウィンドウで 'colcon build' succesfull と表示されます

6. 設定ファイルの反映

アンダーレイ設定ファイル（ROS2システムの設定）は .bashrc に記述済みとします
※下記が.bashrcに既にある
source /opt/ros/foxy/setup.bash

オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記

.bashrcの編集

$ emacs ~/.bashrc

$ source ~/teleop_ws/install/setup.bash
を追加してから反映させる

$ source ~/.bashrc

7. ノードの実行（実機）

今回はロボットを動かすプログラムなのでロボットを起動します．
TurtleBot3の設定については別記事参照．
https://qiita.com/pez/items/1d3d15b3911d5dab1702

TurtleBot3にて

$ ros2 launch turtlebot3_bringup robot.launch.py

リモートPCにて

$ ros2 run my_gamepad my_gamepad_node

左ジョイスティックの動作ですぐに動き出しますので周囲に注意．
止めたいときはそれ以外のボタンを押してみましょう．

8. ノードの実行（シミュレーション）

同じことをシミュレーション上で行うこともできます．
シミュレーションに関するセットアップは別記事参照
https://qiita.com/pez/items/d795978c5436a87bc1b7

gazeboでTurtlebot3のシミュレーション

$ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

別のターミナルにて

$ ros2 run my_gamepad my_gamepad_node

TurtleBot3の実機を立ち上げている状態でさらにシミュレーションも立ち上げてTeleopを行うと，実機もシミュレーションのロボットも両方同時に動きます．（デジタルツイン？）

9. 練習

my_gamepad_node.py を編集してロボットの操作がしやすいように改良を加えてみましょう．
下記を行う必要があります．

　1.my_gamepad_node.pyの中身を編集して保存
　2.ビルド
　3.設定ファイルの反映
　4.ノードの実行