ROSでtfからロボットの自己位置を取得するスクリプト

背景

ROSで自律走行ロボットを作っていて定期的にロボット位置をcsvに書き出したかったので自分用にメモ。

実装

ロボットの原点をmapフレームから見た値に変換

# !/usr/bin/env python
# -*- coding: utf-8 -*-

from __future__ import print_function

import rospy
import tf2_ros
import csv
import tf.transformations as T
import tf2_geometry_msgs

file_path="保存先"

if __name__ == '__main__':
    rospy.init_node('get_robot_pose')
    tfBuffer = tf2_ros.Buffer()
    listerner = tf2_ros.TransformListener(tfBuffer)

    while not rospy.is_shutdown():
        robot_pose = tf2_geometry_msgs.PoseStamped()
        robot_pose.header.frame_id = "base_link"
        robot_pose.header.stamp = rospy.Time(0)
        # Orientationも変換したい場合はwを1.0にする。
        robot_pose.pose.orientation.w = 1.0

        try:
            # base_linkフレームの原点をmapフレーム上の座標に変換
            global_pose = tfBuffer.transform(robot_pose, "map")
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            print("WARNING: tf map to base_link not found.")
            rospy.sleep(1)
            continue

        print("Success getting tf!")

        quaternion_rotation = [
            global_pose.pose.orientation.x, global_pose.pose.orientation.y, global_pose.pose.orientation.z, global_pose.pose.orientation.w
        ]
        euler_rotation = T.euler_from_quaternion(quaternion_rotation)
        with open(full_pose_file_path, 'a') as f:
            writer = csv.writer(f)
            writer.writerow([
                global_pose.header.stamp,
                global_pose.pose.position.x, global_pose.pose.position.y, global_pose.pose.position.z,
                quaternion_rotation[0], quaternion_rotation[1], quaternion_rotation[2], quaternion_rotation[3],
                euler_rotation[0], euler_rotation[1], euler_rotation[2]
            ])
　　　　　# 取得間隔に合わせて調整
        rospy.sleep(0.1)

実装(間違い)

※ 結果はほぼ同じになるがやっていることは、base_linkフレームの値をmapフレームの値に変換するための変換行列の算出。

# !/usr/bin/python
# -*- coding: utf-8 -*-

from __future__ import print_function

import rospy
import tf2_ros
import csv
import tf.transformations as T

file_path="保存先"

if __name__ == '__main__':
    rospy.init_node('get_robot_pose')
    tfBuffer = tf2_ros.Buffer()
    listerner = tf2_ros.TransformListener(tfBuffer)

    while not rospy.is_shutdown():
        try:
            trans = tfBuffer.lookup_transform("map", "base_link", rospy.Time(0), rospy.Duration(1.0))
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            print("WARNING: tf map to base_link not found.")
            rospy.sleep(1)
            continue

        translation = [
            trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z
        ]
        quaternion_rotation = [
            trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w
        ]
        euler_rotation = T.euler_from_quaternion(quaternion_rotation)
        with open(file_path, 'a') as f:
            writer = csv.writer(f)
            writer.writerow([
                trans.header.stamp,
                translation[0], translation[1], translation[2],
                quaternion_rotation[0], quaternion_rotation[1], quaternion_rotation[2], quaternion_rotation[3],
                euler_rotation[0], euler_rotation[1], euler_rotation[2]
            ])
        # 取得間隔に合わせて調整
        rospy.sleep(0.1)