動的座標変換の基本概念

動的座標変換とは、時間とともに相対位置が変化する二つの座標系間の関係を表します。例えば、ロボットアームのエンドエフェクターとベースリンク間や、移動ロボットのベースリンクとワールド座標系間の関係がこれに該当します。静的座標変換が動的座標変換の時間微分と考えることができます。

ここでは、ROSの`turtlesim`シミュレーターを使用して、ワールド座標系に対するロボットの座標をTFで公開する方法を実装します。

C++による実装

まず、作成したパッケージの`src`ディレクトリ内に`coordinate_publisher.cpp`と`coordinate_subscriber.cpp`を作成し、`CMakeLists.txt`に以下を追加します：

add_executable(dynamic_tf_publisher src/coordinate_publisher.cpp)
add_executable(dynamic_tf_subscriber src/coordinate_subscriber.cpp)

target_link_libraries(dynamic_tf_publisher
  ${catkin_LIBRARIES}
)

target_link_libraries(dynamic_tf_subscriber
  ${catkin_LIBRARIES}
)

coordinate_publisher.cppは、子座標系から親座標系への動的座標関係を公開する実装です：

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void poseCallback(const turtlesim::Pose::ConstPtr ¤t_pose)
{
    static tf2_ros::TransformBroadcaster tf_broadcaster;
    
    geometry_msgs::TransformStamped transform_data;
    transform_data.header.frame_id = "world";
    transform_data.header.stamp = ros::Time::now();
    transform_data.child_frame_id = "turtle1";
    
    transform_data.transform.translation.x = current_pose->x;
    transform_data.transform.translation.y = current_pose->y;
    transform_data.transform.translation.z = 0.0;
    
    tf2::Quaternion rotation;
    rotation.setRPY(0, 0, current_pose->theta);
    transform_data.transform.rotation.x = rotation.getX();
    transform_data.transform.rotation.y = rotation.getY();
    transform_data.transform.rotation.z = rotation.getZ();
    transform_data.transform.rotation.w = rotation.getW();
    
    tf_broadcaster.sendTransform(transform_data);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "dynamic_tf_publisher");
    ros::NodeHandle node_handle;
    
    ros::Subscriber pose_subscriber = node_handle.subscribe("/turtle1/pose", 1000, poseCallback);
    
    ros::spin();
    return 0;
}

coordinate_subscriber.cppは、動的座標変換関係を購読し、その関係を使用してカメ座標系からワールド座標系への座標変換を行います：

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "dynamic_tf_subscriber");
    ros::NodeHandle node_handle;
    
    tf2_ros::Buffer tf_buffer;
    tf2_ros::TransformListener tf_listener(tf_buffer);
    
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        geometry_msgs::PointStamped turtle_point;
        turtle_point.header.frame_id = "turtle1";
        turtle_point.header.stamp = ros::Time();
        turtle_point.point.x = 1;
        turtle_point.point.y = 1;
        turtle_point.point.z = 0;
        
        try
        {
            geometry_msgs::PointStamped world_point;
            world_point = tf_buffer.transform(turtle_point, "world");
            ROS_INFO("変換後の座標: (%.2f, %.2f, %.2f), フレーム: %s", 
                world_point.point.x, 
                world_point.point.y, 
                world_point.point.z,
                world_point.header.frame_id.c_str());
        }
        catch (const std::exception &e)
        {
            ROS_ERROR("変換エラー: %s", e.what());
        }
        
        loop_rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}

コンパイル後、以下の手順で実行します：

1. タートルシミュレーターを起動: rosrun turtlesim turtlesim_node
2. 座標変換の公開を開始: rosrun tf2_learning dynamic_tf_publisher
3. RVizを起動: rviz
4. RVizでFixed Frameをworldに設定
5. 左下のAddボタンをクリックし、TFコンポーネントを選択して座標関係を表示
6. 座標変換の購読を開始: rosrun tf2_learning dynamic_tf_subscriber
7. キーボード制御を起動: rosrun turtlesim turtle_teleop_key

キーボードでタートルを移動させると、RVizの表示と変換後の座標が動的に変化することが確認できます。

Pythonによる実装

パッケージの`src`ディレクトリと同じ階層に`scripts`ディレクトリを作成し、そこにPythonスクリプトを保存します。まず座標公開用のtf_broadcaster.pyを作成します：

#!/usr/bin/env python

import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped

def handle_turtle_pose(msg):
    broadcaster = tf2_ros.TransformBroadcaster()
    
    transform = TransformStamped()
    transform.header.frame_id = "world"
    transform.header.stamp = rospy.Time.now()
    transform.child_frame_id = "turtle1"
    
    transform.transform.translation.x = msg.x
    transform.transform.translation.y = msg.y
    transform.transform.translation.z = 0.0
    
    quaternion = tf.transformations.quaternion_from_euler(0, 0, msg.theta)
    transform.transform.rotation.x = quaternion[0]
    transform.transform.rotation.y = quaternion[1]
    transform.transform.rotation.z = quaternion[2]
    transform.transform.rotation.w = quaternion[3]
    
    broadcaster.sendTransform(transform)

if __name__ == "__main__":
    rospy.init_node("dynamic_tf_broadcaster_py")
    rospy.Subscriber("/turtle1/pose", Pose, handle_turtle_pose)
    rospy.spin()

次に、座標変換購読用のtf_listener.pyを作成します：

#!/usr/bin/env python

import rospy
import tf2_ros
from tf2_geometry_msgs import PointStamped

if __name__ == "__main__":
    rospy.init_node("dynamic_tf_listener_py")
    
    tf_buffer = tf2_ros.Buffer()
    tf_listener = tf2_ros.TransformListener(tf_buffer)
    
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        source_point = PointStamped()
        source_point.header.frame_id = "turtle1"
        source_point.header.stamp = rospy.Time.now()
        source_point.point.x = 10
        source_point.point.y = 2
        source_point.point.z = 3
        
        try:
            target_point = tf_buffer.transform(source_point, "world", rospy.Duration(1))
            rospy.loginfo("変換後の座標: (%.2f, %.2f, %.2f), フレーム: %s",
                         target_point.point.x,
                         target_point.point.y,
                         target_point.point.z,
                         target_point.header.frame_id)
        except Exception as e:
            rospy.logerr("変換エラー: %s", e)
        
        rate.sleep()