えいあーるれいの技術日記

ROS2やM5 Stack、Ubuntuについて書いています

自己位置推定してずっと同じ方向を向くやつを作った（rclpy+tf2+T265）

ROS2 ちょっとAIなロボット

　https://ar-ray.hatenablog.com/entry/2021/04/03/180000の続きです。

tf2とは

　※曖昧な部分が多いので、ROSWikiを見るのが確実だと思います。

　tf2とは、簡単に言うとシミュレーションにも対応するロボットの座標変換ライブラリです。ROSの中でも重要なライブラリであると同時に初見殺しがひどいライブラリです（個人的な感想）。tf2はC++やPythonで扱うことができます。

　また、tfはトピック通信と異なり、同じ時系列のデータをリンクさせてデータを取得する必要があるため、ブロードキャスタとリスナが用いられます。tfが持つ情報はタイムスタンプ、リンクの親子関係、相対座標があります。座標については、平行移動にVector3、回転移動にQuaternionを用います。

実現したいこと

　前回使用したロボットについて、T265のオドメトリをもとにメカナムホイールの中心を求めて自己位置推定をします。

　さて、自己位置推定したい場合、どの位置関係を利用するのがよさそうでしょうか？？

f:id:Ray_ar:20210326132449p:plain

　Rviz2では、odom_frameから見たときにロボットの移動が確認できたので、Rviz2で確認した視点と同じ場所から確認すればbase_link(=ロボット)の移動を確認できそうです。つまり、odom_frame→base_linkの相対位置を求めればよいということになります。

f:id:Ray_ar:20210326140206p:plain

　この位置関係を取得して位置を求めればよさそうです。次にアルゴリズムのブロック図を示します。

f:id:Ray_ar:20210327094438p:plain

　これをPythonで記述していきます。現段階では理解が十分でないので、適宜修正してく予定です。

github.com

　抜粋して説明します。

#tf2twsistクラス内
def __init__(self):

    _node = Node("tf2twist")

    qos_profile = QoSProfile(depth=10)
      
    tfBuffer = Buffer()
    listener = TransformListener(tfBuffer,_node, qos=qos_profile)

    twist_data = geometry_msgs.msg.Twist()

    pub_cmd_vel = _node.create_publisher(geometry_msgs.msg.Twist, "cmd_vel", qos_profile)

　whileループ前はモジュールの宣言を行います。ここでは、tfの時間を格納するBufferとリスナ、cmd_velの出力になるTwistの宣言を行いました。

#tf2twsistクラス内
while rclpy.ok():
    rclpy.spin_once(_node)

        now = _node.get_clock().now() - rclpy.duration.Duration(seconds=0,nanoseconds=1000000)
        try:
                trans = tfBuffer.lookup_transform('odom_frame', 'base_link', now, rclpy.duration.Duration(seconds=0, nanoseconds= 0))
            
                roll, pitch, yaw = self.quaternion_to_euler(trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w)

                if(yaw> 0.3):
                        twist_data.angular.z = 70.0#yaw*125*1.3
                elif(yaw< -0.3):
                        twist_data.angular.z = -70.0#yaw*125*1.3
                else:
                        twist_data.angular.z = 0.0
                pub_cmd_vel.publish(twist_data)

　while内では取得した相対座標をもとにロボットの回転を算出します。tfBuffer.lookup.transformで座標を取得します。この時、時間nowは現在の時刻よりも前にする必要があります。（この時はエラーの原因が分からず、やけくそで1000000にしてしまっています。）取得したら、後述する関数でクオータニオンをオイラー角に変換してそのうちangular.zをTwistに格納してパブリッシュします。

def quaternion_to_euler(self, q0, q1, q2, q3):
    q0q0 = q0 * q0
    q0q1 = q0 * q1
    q0q2 = q0 * q2
    q0q3 = q0 * q3
    q1q1 = q1 * q1
    q1q2 = q1 * q2
    q1q3 = q1 * q3
    q2q2 = q2 * q2
    q2q3 = q2 * q3
    q3q3 = q3 * q3

    a = math.atan2(2.0 * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3)
    b = math.asin(2.0 * (q0q2 - q1q3))
    c = math.atan2(2.0 * (q2q3 + q0q1), q0q0 - q1q1 - q2q2 + q3q3)

    return a , b, c

　クオータニオンをオイラー角に変換する関数です。オイラー角とクォータニオン間の変換とベクトルの回転を参考にコードを記述しました。本来はc，b，aの順でroll，pitch，yawだったと思いますが、実在のロボットの位相にそろえるために、a=roll，b=pitch，c=yawで返す関数になっています。

　tf2+rclpyの記事はとても少ないと感じているので、進捗があればちまちまと書いていきます。