はじめに

Gazeboシミュレータでturtlebot3を動かします．
初めからある「teleop_keyboard」と同じ動きをするプログラムを自分で作成する演習です．

環境

・OS: Windows11 Pro (64bit)
・ROS: ROS2 humble (WSL2のUbuntu22.04に)

ここまでの環境構築については下記参照
・WSL2にROS2を入れてgazeboでシミュレーションするまでの手順
https://qiita.com/pez/items/16011df0e663ceac694a

1. ワークスペース作成

ubuntuを立ち上げて作業開始．
ワークスペース名は teleop_ws としておきます．

cd
mkdir -p ~/teleop_ws/src/

2. パッケージ作成

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

cd ~/teleop_ws/src
ros2 pkg create --build-type ament_python --node-name my_teleop_node my_teleop

3. プログラミング

1. package.xml ファイルの編集

※以降「emacs」の部分は自分の好きなエディタを使って下さい．

cd ~/teleop_ws/src/my_teleop
emacs package.xml

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

2. setup.py ファイルの編集

cd ~/teleop_ws/src/my_teleop
emacs setup.py

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

entry_point={
    'console_scripts': [
        'my_teleop_node = my_teleop.my_teleop_node:main' 
    ],
},

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

3.ソースコード作成

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

cd ~/teleop_ws/src/my_teleop/my_teleop
emacs my_teleop_node.py

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

my_teleop_node.py

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


class MyTeleop(Node):  # キー操作により速度指令値をパブリッシュするクラス
    def __init__(self):   # コンストラクタ
        super().__init__('my_teleop_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

    def timer_callback(self):  # タイマーのコールバック関数
        key = input('f, b, r, l, sキー入力後にEnterキーを押下 <<')  # キー取得
        # キーの値により並進速度や角速度を変更
        if key == 'f':
            self.vel.linear.x += 0.1
        elif key == 'b':
            self.vel.linear.x -= 0.1
        elif key == 'l':
            self.vel.angular.z += 0.1
        elif key == 'r':
            self.vel.angular.z -= 0.1
        elif key == 's':
            self.vel.linear.x = 0.0
            self.vel.angular.z = 0.0
        else:
            print('入力キーが違います．')
        self.publisher.publish(self.vel)  # 速度指令メッセージのパブリッシュ
        self.get_logger().info(f'並進速度={self.vel.linear.x} 角速度={self.vel.angular.z}')


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

4. ビルド

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

cd ~/teleop_ws
colcon build

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

5. 設定ファイルの反映

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

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

.bashrcの編集

emacs ~/.bashrc

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

source ~/.bashrc

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

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

TurtleBot3にて

ros2 launch turtlebot3_bringup robot.launch.py

リモートPCにて

ros2 run my_teleop my_teleop_node

f を入力して Enter を押したら走り出します（前進）．周囲に注意．
止める時は s を入力して Enter です．
他の操作も確認してみましょう．

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

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

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

ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

別のターミナルにて

ros2 run my_teleop my_teleop_node

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

8. 演習

演習①

my_teleop_node.py を編集して別の動作がされるようにして下さい
（例）
・押すキーボードの文字を変える
・速度の指令を変更して操作しやすくする
など

下記を行う必要があります．

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

演習②

my_teleop_node.py はこのままだと必ず「Enter」を押さないといけない仕様になっています
「Enter」を押さずに動かせるようにして下さい。