ROS2 勉強⑤（続 プログラミングする）

計算でカメを追尾

今回は２匹のカメをプログラムで動かしてみます。
ロジックを少し複雑にするために、**「カメA（親ガメ）をカメB（子ガメ）が自動で追いかける」**という「追従走行（Following）」のプログラムに挑戦してみましょう。

これには、**「数学（三角関数）」**を使って、相手との距離と方向を計算するロジックが必要になります。

準備：2匹目のカメを出現させる

まずは、追いかけられる側の「カメ2」を画面に出現させましょう。ターミナルから以下のコマンドを打つだけでOKです。

ros2 service call /spawn turtlesim/srv/Spawn "{x: 2.0, y: 2.0, theta: 0.0, name: 'turtle2'}"

これで画面に turtle2 が現れます。

追従ロジックの考え方

カメ1（自分）がカメ2（相手）を追いかけるには、毎秒ごとに以下の計算を行います。

1. 距離の計算: $dist = \sqrt{(x_2 - x_1)^2 + (y_2 - y_1)^2}$
2. 方向の計算: 相手が自分のどの方向にいるかを atan2 関数で求めます。
3. 速度の決定: 距離が遠ければ速く進み、方向がズレていれば急旋回します。

Pythonコードの作成

~/ros2_ws/src/my_robot_controller/my_robot_controller/turtle_follower.py という新しいファイルを作成し、以下のコードを書き込んでください。

turtle_follower.py

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import math

class TurtleFollower(Node):
    def __init__(self):
        super().__init__('turtle_follower')
        # 自分の位置(turtle1)と相手の位置(turtle2)を購読
        self.pose1 = None
        self.pose2 = None
        
        self.sub1 = self.create_subscription(Pose, '/turtle1/pose', self.pose1_callback, 10)
        self.sub2 = self.create_subscription(Pose, '/turtle2/pose', self.pose2_callback, 10)
        
        # 自分の移動命令を送るパブリッシャ
        self.pub = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        self.timer = self.create_timer(0.1, self.control_loop)

    def pose1_callback(self, msg): self.pose1 = msg
    def pose2_callback(self, msg): self.pose2 = msg

    def control_loop(self):
        if self.pose1 is None or self.pose2 is None:
            return

        # 1. 相手との相対距離と角度を計算
        diff_x = self.pose2.x - self.pose1.x
        diff_y = self.pose2.y - self.pose1.y
        distance = math.sqrt(diff_x**2 + diff_y**2)
        
        # 相手の方向（ラジアン）
        target_angle = math.atan2(diff_y, diff_x)
        # 自分の向きとの差分
        angle_error = target_angle - self.pose1.theta
        
        # 角度の補正（-πからπの範囲に収める）
        while angle_error > math.pi: angle_error -= 2.0 * math.pi
        while angle_error < -math.pi: angle_error += 2.0 * math.pi

        msg = Twist()
        if distance > 1.0: # 距離が1.0以上なら追いかける
            msg.linear.x = min(1.5, 0.5 * distance)
            msg.angular.z = 4.0 * angle_error
            if abs(angle_error) > 0.5:   # 約30度以上ズレていたら
                msg.linear.x *= 0.1      # ほぼ止まって向きを直す
        else:
            msg.linear.x = 0.0
            msg.angular.z = 0.0

        self.pub.publish(msg)

def main():
    rclpy.init()
    rclpy.spin(TurtleFollower())
    rclpy.shutdown()

Geminiの解説

このプログラムは、ROS2の応用編として非常に面白い**「追従（フォロー）システム」です。
一言でいうと、「turtle1（自分）が、turtle2（相手）の位置を追いかける」**という、まるで健気なペットのような動きをするアルゴリズムです。

これまでのコードと違い、**「2つの異なる情報（自分と相手の場所）を組み合わせて、次の動きを決める」**のが最大の特徴です。

---

🧭 このプログラムの「作戦」

このコードは、数学（三角関数）を使って「相手がどっちにいて、どれくらい離れているか」を計算しています。

情報を2つ集める（Subscriber）

self.sub1 = self.create_subscription(Pose, '/turtle1/pose', self.pose1_callback, 10)
self.sub2 = self.create_subscription(Pose, '/turtle2/pose', self.pose2_callback, 10)

• /turtle1/pose: 自分の現在地
• /turtle2/pose: 追いかける相手の現在地
• コールバック関数（pose1_callback など）で、届いたデータを self.pose1 などの変数に保存しておきます

数学で「進むべき道」を計算する

control_loop の中身がこのプログラムの心臓部です。

① 距離を測る（三平方の定理）

$$distance = \sqrt{(x_2 - x_1)^2 + (y_2 - y_1)^2}$$

math.sqrt(diff_x**2 + diff_y**2) で、自分と相手が何メートル離れているかを計算します。

② 角度を測る（アークタンジェント）

target_angle = math.atan2(diff_y, diff_x)
相手が自分の「真上」にいるのか「右下」にいるのか、相手がいる方角を割り出します。

③ 自分の向きとの「ズレ」を出す

angle_error = target_angle - self.pose1.theta
「相手がいる方向」から「今の自分の向き」を引き算して、あと何度回転すれば相手の正面を向けるかを計算します。

P制御（比例制御）でなめらかに動かす

ここが最もエンジニアらしい部分です。

msg.linear.x = 1.5 * distance      # 遠いほど速く
msg.angular.z = 6.0 * angle_error  # ズレているほど急旋回

単に「進め」ではなく、**「誤差（距離や角度のズレ）が大きいほど、出力を強くする」**というルールにしています。

• 相手が遠くにいれば猛ダッシュし、近づくとゆっくりになります
• 向きが大きくズレていれば急ハンドルを切り、正面を向いたらハンドルを戻します

📝 注目ポイント：角度の補正（おまじない）

while angle_error > math.pi: angle_error -= 2.0 * math.pi
while angle_error < -math.pi: angle_error += 2.0 * math.pi

これはロボットプログラミングの「あるある」を防ぐコードです。
例えば、自分が +179度 を向いていて、相手が -179度 の方向にいた場合、引き算すると 358度 も回転しようとしてしまいます（ほぼ1回転）。でも実際は、反対側にあと 2度 回るのが最短ルートですよね。
このコードは、常に**「最短距離で相手の方を向く」**ための補正をしています。

🚀 実際に動かすには？

1. まず turtlesim を立ち上げて、１匹目のカメ（turtle1）を表示します
2. turtle2 を出現させる必要があります。（ターミナルから ros2 service call /spawn ... と打つか、別のプログラムで作成します）
3. その状態でこのプログラムを動かすと、turtle1がturtle2に向かって突進していきます
4. 別のターミナルから turtle2 をキーボード操作で動かしてみると、turtle1が一生懸命ついてくるはずです！

💡 試してほしいこと

msg.linear.x = 1.5 * distance の 1.5 という数字（ゲインと呼びます）を 0.5 にしたり 5.0 にしたりすると、追いかけ方の「性格」が変わります。

• 小さくすると、のんびりした動きになります
• 大きくしすぎると、勢い余って相手を通り過ぎ、ガクガクと激しい動きになります

実行してテストする

1. setup.py に新しいスクリプト turtle_follower を登録してビルドします。

~/ros2_ws/src/my_robot_controller/setup.pyのentry_pointsの中に以下の一行を追加します

'turtle_follower = my_robot_controller.turtle_follower:main',

追加の内容は以下の通りです。

• '実行したいコマンド名 = パッケージ名.ファイル名:関数名'
• 実行したいコマンド名: ros2 run の後に打つ名前になります。
• パッケージ名: 今回は my_robot_controller です。
• ファイル名: 作成した turtle_follower.py です（拡張子 .py は抜き）。
• 関数名: プログラムの最後にある def main(): を指します。

追加するときに前の行の最後に「,」を付けるのを忘れずに！

setup.py:修正後のsetup.py

from setuptools import find_packages, setup

package_name = 'my_robot_controller'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='nomura',
    maintainer_email='[e-mail]',
    description='ROS 2 turtle controller and follower',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            # 1匹目のカメのノード（既存）
            'my_turtle_node = my_robot_controller.my_turtle_node:main',
            # 2匹目のカメの追従ノード（今回追加）
            'turtle_follower = my_robot_controller.turtle_follower:main',
        ],
    },
)

cd ~/ros2_ws
colcon build

source install/setup.bash

ros2 run my_robot_controller turtle_follower

2. このプログラムを実行すると、カメ1がカメ2に向かって突進し、手前でピタッと止まります

と、Geminiからは言われたのですが、円を描いていたり、通り過ぎたりで思ったほどうまく動きません…
けっこういろいろと時間がかかって追いつくって感じですね。
ピタッと止まりますって感じではないです。
気になってGeminiでプログラムのアルゴルを確認しましたが、完ぺきなアルゴルと言われましたw

3. 別のターミナルで turtle_teleop_key を turtle2 に対して実行し、カメ2を逃がしてみてください

# turtle2を操作するコマンド
ros2 run turtlesim turtle_teleop_key --ros-args --remap turtle1/cmd_vel:=turtle2/cmd_vel

カメ2をキーボードで動かすと、カメ1が一生懸命追いかけてくるはずです！

これで、「複数の情報を統合して判断を下す」という、かなり高度なプログラムが書けました。

自力計算からTF2へ

ROS 2の最重要機能の一つ、**TF2（Transform Library）**に足を踏み入れます。

これまでは「相手の $x, y$ 座標を引き算して、距離を $\sqrt{x^2+y^2}$ で計算して…」と自力で計算していましたが、TF2を使うと「カメ2から見たカメ1の相対位置を教えて！」と一言頼むだけで、複雑な行列計算をすべてROS 2が裏側で代行してくれます。

---

TF2のイメージ：動く座標系

TF2は、各ロボットに「座標の十字架（フレーム）」を貼り付けるイメージです。

• world: 画面全体の動かない中心点
• turtle1: カメ1と一緒に動く座標系
• turtle2: カメ2と一緒に動く座標系

---

準備：座標情報を「放送」する

TF2を使うには、まず各カメが「今、自分はここにいるよ！」と座標情報を放送（Broadcast）する必要があります。

本来は放送用のコードを書くのですが、まずは手っ取り早く公式のデモパッケージを使って「TF2のすごさ」を体験してみましょう。

必要なパッケージのインストール

sudo apt update
sudo apt install ros-jazzy-turtle-tf2-py ros-jazzy-tf2-ros ros-jazzy-tf2-geometry-msgs

実践：TF2で追いかけっこデモを起動

以下のコマンドを順番に、別々のターミナルで実行してください。

1. デモの起動（カメが2匹出ます）

ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

1. カメ1をキーボードで動かす

ros2 run turtlesim turtle_teleop_key

カメ1を動かすと、カメ2が自動で追いかけてくるはずです。一見先ほどのプログラムと同じですが、中身は数学の計算式を使わず「TF2」に丸投げしています。

座標のつながりを可視化する（view_frames）

今、どんな座標の関係になっているかを図にしてみましょう。

ros2 run tf2_tools view_frames

実行すると frames.pdf というファイルが生成されます。これを開くと、「world → turtle1」や「world → turtle2」というツリー構造が見えるはずです。これがROSロボットの「体の構造」を表す設計図になります。

プログラムでどう書くのか？（概念）

もし自分でコードを書くなら、先ほどの複雑な math.sqrt や atan2 の部分は、たったこれだけになります。

# TF2に「turtle2から見たturtle1の相対位置」を聞く
trans = tf_buffer.lookup_transform('turtle1', 'turtle2', rclpy.time.Time())

# 距離や角度が勝手に出てくる！
dist = math.sqrt(trans.transform.translation.x**2 + trans.transform.translation.y**2)
angle = math.atan2(trans.transform.translation.y, trans.transform.translation.x)

**「数学を意識せず、座標系の名前だけ意識すればいい」**のがTF2のメリットです。

修正後

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import tf2_ros
import math


class TurtleFollower(Node):
    def __init__(self):
        super().__init__('turtle_follower')

        # TF2バッファとリスナーの設定
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)

        # turtle2の移動命令パブリッシャ
        self.pub = self.create_publisher(Twist, '/turtle2/cmd_vel', 10)

        # 制御ループ（10Hz）
        self.timer = self.create_timer(0.1, self.control_loop)

    def control_loop(self):
        try:
            # turtle2座標系から見たturtle1の位置をTF2で取得
            trans = self.tf_buffer.lookup_transform(
                'turtle2',   # ターゲットフレーム（自分＝追いかける側）
                'turtle1',   # ソースフレーム（追いかける相手）
                rclpy.time.Time()
            )
        except (tf2_ros.LookupException,
                tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException) as e:
            self.get_logger().warn(f'TF2取得失敗: {e}')
            return

        # turtle2座標系での距離と角度誤差を計算
        dist = math.sqrt(trans.transform.translation.x**2 + trans.transform.translation.y**2)
        angle = math.atan2(trans.transform.translation.y, trans.transform.translation.x)

        msg = Twist()
        if dist > 0.5:
            # 速度に上限を設ける
            msg.linear.x = min(1.5, 0.5 * dist)

            # 角度ズレが大きいときは回転優先、直進を抑制
            msg.angular.z = 4.0 * angle
            if abs(angle) > 0.5:   # 約30度以上ズレていたら
                msg.linear.x *= 0.1  # ほぼ止まって向きを直す
        else:
            msg.linear.x = 0.0
            msg.angular.z = 0.0

        self.pub.publish(msg)


def main():
    rclpy.init()
    rclpy.spin(TurtleFollower())
    rclpy.shutdown()