ROS2でカメさんを制御してみる

#序章
[前回](https://qiita.com/l1sum/items/78f1a904fff3d389d8be)は、ROS2環境上でGazeboを動作するようにセットアップしました。
そしてついにTurtlebot3がシミュレーション上で動作し、オドメトリも取得できることが確認できました。
今回は実際にTurtlebot3をPython3系用ラッパーであるrclpyライブラリを用いて操作してみることにします。
そして実際にやってみることとして以下の目標を立てました。

- ros2コマンドを利用したカメさんへの命令
- rclpyからカメさんへの命令

技術的には全く難しいものではありません。一応解説を加えています。

#カメさんの制御
Gazeboのセットアップが終わっていない方は[前回](https://qiita.com/l1sum/items/78f1a904fff3d389d8be)の記事を参考に環境を構築してください。
## 1. ros2コマンドを利用したカメさんへの命令
現在ROS2は開発段階ではあるもののROS1との相違点が顕著に現れてきています。
その一つに```ros2```コマンドが挙げられます。
このコマンド一つでかんたんにtopicの監視からパッケージの作成などを実行できます。
###-- 実行 --
まずはgazeboでworldを立ち上げましょう。

```
$ gazebo /usr/share/gazebo-9/worlds/turtlebot3_ros2_demo.world
```
これで立ち上がったはずです。カメさんのポジションは初期位置でお願いします。
では```ros2```コマンドで制御してみます。(これは面白くない)

```
$ ros2 topic pub /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1}, angular: {z: 0.3}}'
```
カメさんが回転し始めたはずです。
###-- コマンド引数解説 --
では```ros2```コマンドの引数を解説をします。

```topic``` &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;トピックに関する動作を指示します。この引数によりデータのやり取りをコマンドラインから行うことができます。
```pub``` &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;データを送信することができます。適切なデータ型を選ぶことでロボットを制御することができます。(Bashからも制御可能...)
```/cmd_vel``` topic名を示しています。今回はTurtlebot3を動かすために必要でした。また、topic名の監視は

```
$ ros2 topic list
```
で確認することができます。
```geometry_msgs/Twist```
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;データの型を示しています。ロボットを操作する上ではこの型が主流のはずです。
```'{linear: {x: 0.1}, angular: {z: 0.3}}'```
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;そしてこれが実際のデータです。これはx軸上に0.1[m/s]の速度で直線運動、z軸中心に0.3[rad/s]で回転運動せよという意味になっています。
###-- 結果 --
コマンドからをロボットを操作する上で重要なのは
1. ノード名を適切に設定する
2. 型を間違えない (geometry_msgs/Twist)
3. 適切なデータを渡してあげる
この２つを守れば動きます。

##2. rclpyからカメさんへの命令
上記ではコマンドからTurtlebot3の操作ができました。そして結果を踏まえてPython3系プログラムから操作を試みてみます。
###プログラム
今回作成したプログラムは全く難しいものではありません。とてもシンプルな構造となっています。
まずはプログラムの全体像です。

```:Python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class Controle(Node):
    def __init__(self):
        super().__init__("turtlebot3")

        self.pub = self.create_publisher(Twist, "/cmd_vel")

        self.twist = Twist()

        l = input("Linear  : ")
        a = input("Angular : ")

        self.run(l, a)

    def run(self, l, a):
        self.twist.linear.x = float(l)
        self.twist.angular.z = float(a)

        self.pub.publish(self.twist)

def main():
    rclpy.init()

    node = Controle()

    rclpy.spin(node)

    node.destroy_node()

    rclpy.shutdown()


if __name__ == "__main__":
    main()
```
単純な移動のためだけなのでこのような特徴のないプログラムとなっています。
では解説に入ります。

```
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
```
※rclpyのインポートの目的はここでは説明はしません。main関数の解説時に説明します。
NodeはROSにおける通信部分の重要なパーツです。これがなくては制御ができません。
Twistは速度を扱う場合に多用されます。このクラスにはLinearとAngularが要素として含まれていますが、それぞれ三軸のx,y,zにfloatを渡します。

```
class Controle(Node):
    def __init__(self):
        super().__init__("turtlebot3")

        self.pub = self.create_publisher(Twist, "/cmd_vel")

        self.twist = Twist()

        l = input("Linear  : ")
        a = input("Angular : ")

        self.run(l, a)

    def run(self, l, a):
        self.twist.linear.x = float(l)
        self.twist.angular.z = float(a)

        self.pub.publish(self.twist)
```
この部分ではTurtlebot3の/cmd_velに実際に速度を渡しています。
最も重要なのは

```
self.pub = self.create_publisher(Twist, "/cmd_vel")
```
になります。データを送信するためpublisherというものを作成しますが、データの型(第一引数)と送信先のtopic名(第二引数)が一致していないといけません。

```
self.pub.publish(self.twist)
```
そして作成されたpublisherを用いて、データを送信します。
次はmain関数内です。

```
def main():
    rclpy.init()

    node = Controle()

    rclpy.spin(node)

    node.destroy_node()

    rclpy.shutdown()
```
rclpyの役割は3つしかありません。
rclの初期化、実行の継続、rclの終了処理です。
(プログラムの解説が難しい...)

###-- 実行 --
ではGazeboを起動し、プログラムを実行してみましょう。
今回はLinear.xには0.3、Angular.zには0.5を指定しました。
以下が実行結果になります。
![out.gif](https://qiita-image-store.s3.amazonaws.com/0/238812/6a5c9ad7-a963-97d5-746c-9dcb675539f2.gif)
Linear.xにはもう少し小さい値が適切でしょう。(0.1ぐらい)

###-- 結果 --
ロボットを動かすだけであれば何も苦労することはありません。
意識するべきことは
1. 的確なデータ型とトピック名を指定する。
2. 設定した通りにデータを渡す。
この2つに限ると思います。

#結論
今回は制御を行う上での基礎であるロボットの操作を行いました。
これが理解できればシミュレーション上で好きなロボットを操作することができます。
参考になれば幸いです。お疲れ様でした。

#次回
/scanを受信して遊んでみる。