tf

뭔가 쓸일이 생길거 같아서 정리하는 tf/tutorial

what is tf?

ROS를 사용하는 경우, 메시지 헤더에는 일반적으로 아래의 내용을 포함하고 있다.

• uint32 seq
• time stampe
• string frame_id

여기서 "frame_id"란 좌표계의 이름을 의미한다.
예를 들어, UGV에 라이다 센서를 올려서 주행한다고 하면, 라이다 데이터는 라이다의 설치위치를 중심으로 설정된 좌표계로 데이터를 받는다. 그렇다면 UGV 좌표계를 기준으로 점군 데이터가 어떻게 나오는지 표현하고 싶다면 어떻게 해야할까나??
즉, 각 좌표계 사이의 관계를 정의할 필요가 있으며 이를 tf라고 한다.

(좌표계가 이모양이면 서로의 관계를 어떻게 정의할 것인가..??!?!?!)

Writing a tf broadcaster

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void pose_callback(cosnt turtlesim::PoseConstPtr &msg)
{
  // 변환 정보를 전송할 TransformBroadcaster 객체 생성
  static tf::TransformBroadcaster br;
  // 변환 객체 생성 및 2D pose 정보를 3D pose에 복사
  tf::Transform transform;
  transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
  tf::Quaternion q;
  q.setRPY(0,0, msg->theta);
  // transform 객체에 rotation 설정
  transform.setRotation(q);
  // TransformBroadcaster로 변환정보 전달
  // publishing 하는 변환정보에 타임스태프 설정(ros::Time::now())
  // parent frmae의 이름 전달("world")
  // child frame의 이름 전달(turtle_name)
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "my_tf_broadcaster");
  if(argc != 2)
  {
    ROS_ERROR("need turtle name as argument");
    return -1;
  }
  turtle_name = argv[1];

  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name + "/pose", 10, &pose_callback);

  ros::spin();
  return 0;
}

Writing a tf listener

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_tf_listener");

    ros::NodeHandle node;

    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (node.ok())
    {
        tf::StampedTransform transform;
        try
        {
            // /turtle1에서 /turtle2로의 변환을 원함
            // ros::TIme(0)로 최신 transform 취득
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        // /turtle1의 거리와 각도를 /turtle2의 새로운 선형과 각속도를 계산
        // 새로 계산된 속도는 turtle2/cmd_vel로 publish
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);

        rate.sleep();
    }
    return 0;
};

Adding a frame

하나의 parent frame에는 여러 child frame을 가질 수 있다.

예를 들어, world frame에는 turtle1, turtle2 두개의 child frame을 가지고 있고, turtle1은 carrot1이라는 child frame을 가지고 있다.

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;

  tf::TransformBroadcaster br;
  tf::Transform transform;

  ros::Rate rate(10.0);
  while (node.ok())
  {
    // parent turtle1에서 new child carrot1으로의 변환.
    // carrot1은 turtle1 frame을 기준, 왼쪽으로 2m offset한 위치에 있다.
    transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
    rate.sleep();
  }
  return 0;
};

Learning about tf and time

lookupTransform()함수를 사용하면, 최신의 변환을 알 수 있지만 변환이 기록된 시간을 알지는 못한다.
tf_listener 예제에서 보면,

   try{
      listener.lookupTransform("/turtle2", "/turtle1",  
                               ros::Time(0), transform);

ros::Time(0)은 "가장 최근에 사용 가능한" 변환을 의미.

  try{
    listener.lookupTransform("/turtle2", "/turtle1",  
                             ros::Time::now(), transform);

ros::Time::now()를 통하여 현재 시간의 변환을 얻을 수 있다.

  try{
    ros::Time now = ros::Time::now();
    listener.waitForTransform("/turtle2", "/turtle1",
                              now, ros::Duration(3.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             now, transform);

waitForTransform()함수를 사용하면, 지정 시간만큼 transform을 기다린다.