[ROS2]tf에 대해서 알아봅시다-1

제가 처음에 ros_control이라거나 gazebo, rviz등을 접했을 때는 urdf로 모델을 만들었기때문에 tf랑 직접 마주칠일이 없었는데 요즘 일때문에 SLAM을 다루다 보니까 tf를 모르면 일이 되지 않더군요. 그래서 한번 짚어봅니다.

 

자료는 ROS2의 자습서를 참고로 했으니까 소스코드는 따로 업로드하지 않겠습니다.

이 게시물에선 소스코드의 설명을 위주로 하니, 빌드스크립트는 원 게시물인 자습서를 참고로 작성해주세요.

 

TF란 뭘까?

 

어떤 점에서 프레임과 프레임의 상대적 위치를 표현하는 변환입니다.

예를들어, 자습서에 자주나오는 turtle_sim에서는 거북이의 위치,각도를 world와의 상대적인 위치/방위로 표현합니다.

world가 (0,0,0)이고, 기본적인 방위는 (1,0,0,0)이라고 하고(이건 기본값입니다.), 거북이가 (1,1,0)에 위치하고, 방위가 x축에서 반시계로 60도 틀었다면, 거북이를 나타내는 표현방법은 translation이 (1,1,0)이고 rotation이 (0, 0, sin(pi/6), cos(pi/6))이 됩니다.

rotation이나 방위가 이상하게 숫자가 네개가 나오고, 60도 틀었는데 왜 pi/6이 되는지 궁금하신다면 우선은 사원수(quaternion)에 대해서 자료를 조사해보는 것을 추천합니다.

 

특성상, 단일 노드가 아닌 ros내의 다수의 노드에서 접근하는 경우가 많습니다. 예를 들자면, ros_control에서 joint를 돌리면 그에 맞춰서 링크들의 위치와 방위가 변하니 tf가 바뀔 수밖에 없고, 시뮬레이션으로 화면에 출력하려고 한다면 rviz나 gazebo등에서도 이 정보가 필요합니다. 따라서, 주기적으로 tf를 관리하는 노드가 알아서 주기적으로 publish해줍니다.

 

 

TF의 broadcaster을 만들어보자

 

https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.html

Writing a broadcaster (C++) — ROS 2 Documentation: Humble documentation

을 번역했습니다. 소스도 여기에 있어요.

 

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <turtlesim/msg/pose.hpp>

#include <memory>
#include <string>

using std::placeholders::_1;

class FramePublisher : public rclcpp::Node
{
public:
  FramePublisher()
  : Node("turtle_tf2_frame_publisher")
  {
    // Declare and acquire `turtlename` parameter
    this->declare_parameter<std::string>("turtlename", "turtle");
    this->get_parameter("turtlename", turtlename_);

    // Initialize the transform broadcaster
    tf_broadcaster_ =
      std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
    // callback function on each message
    std::ostringstream stream;
    stream << "/" << turtlename_.c_str() << "/pose";
    std::string topic_name = stream.str();

    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&FramePublisher::handle_turtle_pose, this, _1));
  }

private:
  void handle_turtle_pose(const turtlesim::msg::Pose & msg)
  {
    rclcpp::Time now = this->get_clock()->now();
    geometry_msgs::msg::TransformStamped t;

    // Read message content and assign it to
    // corresponding tf variables
    t.header.stamp = now;
    t.header.frame_id = "world";
    t.child_frame_id = turtlename_.c_str();

    // Turtle only exists in 2D, thus we get x and y translation
    // coordinates from the message and set the z coordinate to 0
    t.transform.translation.x = msg.x;
    t.transform.translation.y = msg.y;
    t.transform.translation.z = 0.0;

    // For the same reason, turtle can only rotate around one axis
    // and this why we set rotation in x and y to 0 and obtain
    // rotation in z axis from the message
    tf2::Quaternion q;
    q.setRPY(0, 0, msg.theta);
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    // Send the transformation
    tf_broadcaster_->sendTransform(t);
  }
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  std::string turtlename_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FramePublisher>());
  rclcpp::shutdown();
  return 0;
}

우선 노드오브젝트의 생성자부터 봅시다.

FramePublisher()
  : Node("turtle_tf2_frame_publisher")
  {
    // Declare and acquire `turtlename` parameter
    this->declare_parameter<std::string>("turtlename", "turtle");
    this->get_parameter("turtlename", turtlename_);

    // Initialize the transform broadcaster
    tf_broadcaster_ =
      std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
    // callback function on each message
    std::ostringstream stream;
    stream << "/" << turtlename_.c_str() << "/pose";
    std::string topic_name = stream.str();

    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&FramePublisher::handle_turtle_pose, this, _1));
  }

주석에 따라서 세 단락에 대해서 설명을 적자면,

1. 우선 패러미터서버에서 turtlename라는 패러미터를 turtle라는 값으로 선언, 정의하고 그 값을 읽어와서 turtlename_에 복사합니다. 이거만 보면 turtle라는 값이 복사되겠구나라고 생각하겠지만, 패러미터 서버에 이미 정의되어있다면 turtle라고 덮어쓰기를 하지 않기에, 이미 정의되어있는 값을 가져옵니다. 주로 이런 경우는 런치파일에서 실행옵션으로 정의해주는 경우가 많습니다.

2. transform broadcaster오브젝트를 만듭니다.

생성자로 *this를 넘겨주는 것을 보면 노드 핸들을 자기가 갖고있어야 하는 모양이군요.

3. 어떤 토픽에 대한 subscriber를 만듭니다.

토픽명의 일부를 turtlename_에서 가져와서 만들 것이기 때문에 stringstream을 갖다 썼네요. 저같으면 std::string의 +오퍼레이터를 적당히 갖다쓰는 것도 괜찮지 않았을까 생각하지만요.  아무튼 <turtlesim::msg::Pose>타입의 메시지를 구독하는 subscriber을 만듭니다. 토픽명은 "/{turtlename_}/pose"가 되겠구요. subscribe 콜백함수는 이어서 적겠습니다.

void handle_turtle_pose(const turtlesim::msg::Pose & msg)
  {
    rclcpp::Time now = this->get_clock()->now();
    geometry_msgs::msg::TransformStamped t;

    // Read message content and assign it to
    // corresponding tf variables
    t.header.stamp = now;
    t.header.frame_id = "world";
    t.child_frame_id = turtlename_.c_str();

    // Turtle only exists in 2D, thus we get x and y translation
    // coordinates from the message and set the z coordinate to 0
    t.transform.translation.x = msg.x;
    t.transform.translation.y = msg.y;
    t.transform.translation.z = 0.0;

    // For the same reason, turtle can only rotate around one axis
    // and this why we set rotation in x and y to 0 and obtain
    // rotation in z axis from the message
    tf2::Quaternion q;
    q.setRPY(0, 0, msg.theta);
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    // Send the transformation
    tf_broadcaster_->sendTransform(t);
  }

맨 마지막줄을 보면 broadcaster의 어떤 메소드에다가 geometry_msgs::msg::TransformStamped타입의 객체를 전달하네요. 함수명을 보니까 이게 tf를 전송하는 것 같구요. 이 객체의 명세를 구글링해서 찾아보면 

std_msgs/Header header
string child_frame_id
geometry_msgs/Transform transform

라고 나옵니다 header는 시간header.stamp, 이 메세지의 주체header.frame_id를 갖고있어야되고 child_frame_id로 목적지의 frame를 줘야합니다. 또 geometry_msgs/Transform타입의 transform을 전달해야하는데, 이는 translation과 rotation으로 나뉘어져서, 각각 위치벡터와 회전에 관한 사원수를 전달해줘야 합니다.

 

 이정도 명세만 알면 나머지는 대입입니다. 주석에 따라서 단락을 하나씩 나눠주면

1. 시간과 어디에서 어디로 향하는 변환인지 명시해줍니다. 시간은 바로 지금이고, 변환은 world프레임에서 turtle_name프레임으로 향하는 변환이네요.

2. translation을 정의해줍니다. 여기서부턴 메시지의 내용을 참조하기때문에, 메시지가 어떻게 생겼는지 뜯어보겠습니다. turtle_sim::msg::pose메세지의 명세는 이렇게 나옵니다.

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

딱봐도 x,y는 위치좌표인거같고, theta는 어느축을 기준으로 틀었는지는 모르겠지만(사실은 x축에서 반시계방향으로 틀은 각도수입니다.) 어쨌든 향하는 방향같네요.

"world"프레임에서 "turtle_"프레임으로 향하는 변환(tf)를 만들거니까, world프레임을 원점(0,0,0)이라고 한다면, 그 위치벡터는 turtle_의 좌표가 되고, 이게 바로 translation에 지정됩니다.

3. rotation을 정의해줍니다.  오른손좌표계라고 생각했을때 x축에서 반시계로 회전시킨다는것은, z축을 축으로 반시계방향으로 회전시킨다는 말과 같습니다. 이는 RPY-Roll, Pitch, Yaw에서 Yaw에 해당합니다. 따라서, 임의의 사원수오브젝트(tf2::Quaternion)을 만들고, 이것의 값을 Eucledean rotation-RPY에서 Yaw값을 지정해주고, 이를 사원수로 변환하라고 지시합니다.

 

메시지의 명세에서 요구되는 Header정보, child_frame_id("turtle_")와 Transform정보(translation, rotation)을 완성했으니, broadcaster 오브젝트의 sendTransform에 전달합니다.

 

마지막으로, 수신할려는 토픽명에 쓰였던 "turtle_"의 정체를 확인하고 싶은데요, 이는 런치스크립트에 적혀있습니다.

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_cpp',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
    ])

런치파일에서는 turtlesim 패키지의 turtlesim_node실행파일을 sim이라는 노드명으로 실행하고, 그 다음에는 learning_tf2_cpp패키지의 turtle_tf2_broadcaster실행파일을 broadcaster1이라는 노드명으로 실행하네요. 그 다음에 turtle_가 읽어들이려는 turtlename패러미터의 정체가 나타나네요. turtle1입니다. 그렇다면 수신하려는 토픽명은 "/turtle1/pose"가 되겠네요.

 

우선 실행하고 rqt_graph를 한번 봅시다.

런치파일에서 지정한대로  sim이라는 노드명에서 거북이의 위치,방위정보를 발행하고, 이게 broadcaster1노드에서 제대로 수신하고있네요.

혹시 안보이는 topic가 있나 토픽들을 확인해봅시다.

/tf라는 토픽이 보이네요.

맨 처음에 언급했다시피, 로봇에는 프레임(부위)가 여러 개가 있을 것을 상정하기때문에, /tf라는 토픽에서 프레임간의 tf들을 일괄적으로 관리합니다. 그렇다면, 아까 world프레임에서 turtle1프레임으로 향하는 위치벡터를 메시지로 발행했는데, 이를 확인할 수 있는 방법은 없을까요?

우선 모든 프레임들의 연결관계를 확인해봅시다.

ros2 run tf2_tools view_frames

라고 명령어를 입력하면, 현재 워크스페이스의 루트폴더에 pdf파일이 하나 생성됩니다. 이걸 열어보면

이렇게 생긴 문서가 나옵니다. 앞서 broadcaster에다가 world프레임에서 turtle_(turtle1)프레임으로의 변환(tf)를 발행하도록 했는데, 그대로 나오는군요. 참고로 average rate가 62.694인 이유는 turtlesim에서 발행하는 위치, 방위정보의 발행빈도때문입니다.

그렇다면, tf의 내용-translation, rotation은 어떻게 확인할까요?

ros2 run tf2_ros tf2_echo world turtle1

tf2_ros패키지의 tf2_echo를 실행시키고 인수로 world과 turtle1을 줍니다. 각각 출발점과 목적점에 해당합니다.

런치스크립트를 실행시키고 거북이한테 아무런 명령도 내리지 않았으니 거북이는 원점에서 x축의 양의 방향을 향하고 있습니다.

그러니 translation은 5.54, 5.54,0이고, RPY는 0,0,0을 가리키고 있습니다. 사원수의 0,0,0,1은 아무런 회전도 시키지 않았음을 의미합니다.

 

여기서, 저는 거북이를 반시계방향으로 조금 회전시키고 앞으로 조금 전진시켜보겠습니다

translation의 x,y값이 아까전의 값보다 조금씩 늘어나고, RPY값도 변했네요.

오른손 좌표계에서 z축으로 반시계회전시켰으니 양의 방향으로 늘어난 것을 확인할 수 있습니다.

 

이상, 요약하면

1. ros2 run turtlesim turtle_teleop_keytf를 발행하는 방법: 출발프레임, 목표프레임의 이름을 문자열로 지정하고, 출발프레임을 목표프레임으로 변환할때 필요한 위치벡터, 회전사원수를 각각 translation, rotation으로 만들어서 메시지 오브젝틀 만든 후, broadcaster의 send메소드에다가 던진다.

2. 내용을 확인하는 방법: tf2_tools view frames로 tf노드가 발행하고있는 프레임간의 tf정보를 확인할 수 있다. 특정 프레임간의 tf의 내용(translation, rotation)을 확인하려면 tf2_ros tf2_echo로 확인할 수 있다.