ROS2プログラミング基礎_1-2: Teleopの自作

## はじめに
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

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


## 1. ワークスペース作成
今回は teleop_ws としておきます．
```ruby:
$ cd
$ mkdir -p ~/teleop_ws/src/
```

## 2. パッケージ作成
今回ノード名は my_teleop_node
パッケージ名は my_teleop
にすることにします
```ruby:
$ cd ~/teleop_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_teleop_node my_teleop
```


## 3. プログラミング
#### 1. package.xml ファイルの編集
```ruby:
$ cd ~/teleop_ws/src/my_teleop
$ emacs package.xml
```
自作パッケージを一般公開するときには変更する必要がりますが，今回は特に何もしなくてOK．

#### 2. setup.py ファイルの編集
```ruby:
$ cd ~/teleop_ws/src/my_teleop
$ emacs setup.py
```
23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要がある．今回パッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっています．
```ruby:
entry_point={
    'console_scripts': [
        'my_teleop_node = my_teleop.my_teleop_node:main' 
    ],
},
```
よって今回何もする必要はありませんが，もし１つのパッケージに複数のPythonファイルがある場合はそのファイルごとにエントリポイントを指定しなければならないので setup.py を必ず変更する必要があります．

#### 3.ソースコード作成
今回もパッケージ作成時に自動的に作られている my_teleop_node.py を編集することにします．
```ruby:
$ cd ~/teleop_ws/src/my_teleop/my_teleop
$ emacs my_teleop_node.py
```
下記のように記述してみましょう．
```ruby: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 ディレクトリが作成されます
```ruby:
$ cd ~/teleop_ws
$ colcon build
```
成功すると小さな別ウィンドウで 'colcon build' succesfull と表示されます

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

オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記
```ruby:.bashrcの編集
$ emacs ~/.bashrc
```
$ source ~/teleop_ws/install/setup.bash
を追加してから反映させる
```ruby:
$ source ~/.bashrc
```

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

```ruby:TurtleBot3にて
$ ros2 launch turtlebot3_bringup robot.launch.py
```

```ruby:リモートPCにて
$ ros2 run my_teleop my_teleop_node
```
f を入力して Enter を押したら走り出します（前進）．周囲に注意．
止める時は s を入力して Enter です．
他の操作も確認してみましょう．

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

```ruby:gazeboでTurtlebot3のシミュレーション
$ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
```
```ruby:別のターミナルにて
$ 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」を押さずに動かせるようにするにはどうしたらよいでしょうか