tf2 정적 브로드캐스터 작성 (C++)

목표: tf2에 정적 좌표 프레임을 게시하는 방법을 배웁니다.

배경

정적 변환을 게시하면 로봇 베이스와 해당 센서 또는 정지 부품 간의 관계를 정의하는 데 유용합니다. 예를 들어, 레이저 스캔 측정 값을 레이저 스캐너 중심에있는 프레임에서 생각하는 것이 가장 쉽습니다.

이것은 정적 변환의 기본을 다루는 독립형 튜토리얼로, 두 부분으로 구성되어 있습니다. 첫 번째 부분에서는 정적 변환을 tf2에 게시하기 위한 코드를 작성합니다. 두 번째 부분에서는 명령 줄 tf2_ros 의 static_transform_publisher 실행 도구를 사용하는 방법을 설명합니다.

다음 두 튜토리얼에서는 tf2 소개 튜토리얼 에서 데모를 재현하는 코드를 작성합니다. 그 후 다음 튜토리얼에서는 더 고급 tf2 기능으로 데모를 확장하는 데 중점을 둡니다.

필수 준비물

이전 튜토리얼에서는 작업 공간을 생성하는 방법 과 패키지를 생성하는 방법 을 배웠습니다.

작업

1 패키지 생성

먼저 이 튜토리얼 및 이후 튜토리얼에 사용될 패키지를 생성합니다. learning_tf2_cpp 라는 패키지는 geometry_msgs, rclcpp, tf2, tf2_ros 및 turtlesim 에 종속되며 다음 튜토리얼에서 사용할 것입니다. 이 튜토리얼의 코드는 여기 에서 찾을 수 있습니다.

새 터미널을 열고 ROS 2 설치 환경을 소스로 지정 하여 ros2 명령을 사용할 수 있도록합니다. 작업 공간의 src 폴더로 이동하고 새 패키지를 만듭니다.

ros2 pkg create --build-type ament_cmake --license Apache-2.0 --dependencies geometry_msgs rclcpp tf2 tf2_ros turtlesim -- learning_tf2_cpp

터미널에서 패키지 learning_tf2_cpp 및 모든 필요한 파일 및 폴더를 생성하는 확인 메시지가 반환됩니다.

2. 정적 변환자 노드 작성

우선 소스 파일을 작성하겠습니다. src/learning_tf2_cpp/src 디렉토리에서 다음 명령을 입력하여 예제 정적 변환자 코드를 다운로드합니다.

Linux

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp

선호하는 텍스트 편집기를 사용하여 파일을 엽니다.

#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

class StaticFramePublisher : public rclcpp::Node
{
public:
  explicit StaticFramePublisher(char * transformation[])
  : Node("static_turtle_tf2_broadcaster")
  {
    tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    // Publish static transforms once at startup
    this->make_transforms(transformation);
  }

private:
  void make_transforms(char * transformation[])
  {
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "world";
    t.child_frame_id = transformation[1];

    t.transform.translation.x = atof(transformation[2]);
    t.transform.translation.y = atof(transformation[3]);
    t.transform.translation.z = atof(transformation[4]);
    tf2::Quaternion q;
    q.setRPY(
      atof(transformation[5]),
      atof(transformation[6]),
      atof(transformation[7]));
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    tf_static_broadcaster_->sendTransform(t);
  }

  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
};

int main(int argc, char * argv[])
{
  auto logger = rclcpp::get_logger("logger");

  // Obtain parameters from command line arguments
  if (argc != 8) {
    RCLCPP_INFO(
      logger, "Invalid number of parameters\nusage: "
      "$ ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster "
      "child_frame_name x y z roll pitch yaw");
    return 1;
  }

  // As the parent frame of the transform is `world`, it is
  // necessary to check that the frame name passed is different
  if (strcmp(argv[1], "world") == 0) {
    RCLCPP_INFO(logger, "Your static turtle name cannot be 'world'");
    return 1;
  }

  // Pass parameters and initialize node
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<StaticFramePublisher>(argv));
  rclcpp::shutdown();
  return 0;
}

2.1 코드 살펴보기

이제 tf2에 정적 거북이 포즈를 게시하는 데 관련된 코드를 살펴보겠습니다. 첫 번째 줄에서 필요한 헤더 파일을 포함합니다. 먼저 TransformStamped 메시지 유형에 액세스하기 위해 geometry_msgs/msg/transform_stamped.hpp 를 포함합니다. 이 메시지를 변환 트리에 게시할 것입니다.

#include "geometry_msgs/msg/transform_stamped.hpp"

그 다음에는 rclcpp 를 포함하여 해당 rclcpp::Node 클래스를 사용합니다.

#include "rclcpp/rclcpp.hpp"

tf2::Quaternion 은 오일러 각도를 쿼터니온으로 변환하거나 그 반대로 변환하는 편리한 함수를 제공하는 쿼터니온을 나타내는 클래스입니다. 또한 tf2_ros/static_transform_broadcaster.h 를 포함하고 StaticTransformBroadcaster 을 사용하여 정적 변환을 쉽게 게시할 수 있도록 합니다.

#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

StaticFramePublisher 클래스 생성자는 이름이 static_turtle_tf2_broadcaster 인 노드를 초기화합니다. 그런 다음 StaticTransformBroadcaster 를 생성하며 시작시 한 번 정적 변환을 보냅니다.

tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

this->make_transforms(transformation);

여기서는 전송될 실제 변환 값을 제공하기 전에 TransformStamped 개체를 생성합니다.

1. 게시할 변환에 타임스탬프를 지정해야하며 현재 시간으로 새로 맞추겠습니다. this->get_clock()->now()

2. 그런 다음 생성 중인 링크의 상위 프레임의 이름을 설정해야합니다. 이 경우 world 입니다.

3. 마지막으로 생성 중인 링크의 하위 프레임의 이름을 설정해야합니다.

geometry_msgs::msg::TransformStamped t;

t.header.stamp = this->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = transformation[1];

여기서 거북이의 6D 포즈 (이동 및 회전)를 채웁니다.

t.transform.translation.x = atof(transformation[2]);
t.transform.translation.y = atof(transformation[3]);
t.transform.translation.z = atof(transformation[4]);
tf2::Quaternion q;
q.setRPY(
  atof(transformation[5]),
  atof(transformation[6]),
  atof(transformation[7]));
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

마지막으로 sendTransform() 함수를 사용하여 정적 변환을 게시합니다.

tf_static_broadcaster_->sendTransform(t);

2.2 package.xml 업데이트

src/learning_tf2_cpp 디렉토리로 이동하여 이미 생성된 CMakeLists.txt 및 package.xml 파일을 확인합니다.

패키지 생성 튜토리얼에서 언급했듯이 <description>, <maintainer>, <license> 태그를 작성해야 합니다.

<description>Learning tf2 with rclcpp</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

파일을 저장하십시오.

2.3 CMakeLists.txt

CMakeLists.txt에 실행 가능한 파일을 추가하고 나중에 ros2 run 에서 사용할 static_turtle_tf2_broadcaster 로 이름을 지정합니다.

add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
   static_turtle_tf2_broadcaster
   geometry_msgs
   rclcpp
   tf2
   tf2_ros
)

마지막으로 install(TARGETS…​) 섹션을 추가하여 ros2 run 에서 실행 가능한 파일을 찾을 수 있도록합니다.

install(TARGETS
   static_turtle_tf2_broadcaster
   DESTINATION lib/${PROJECT_NAME})

3. 빌드

빌드하기 전에 빠진 종속성을 확인하기 위해 워크스페이스의 루트에서 rosdep 를 실행하는 것이 좋습니다.

Linux

rosdep install -i --from-path src --rosdistro humble -y

워크스페이스의 루트에서 새 패키지를 빌드하십시오.

Linux

colcon build --packages-select learning_tf2_cpp

새 터미널을 열고 워크스페이스의 루트로 이동하여 설정 파일을 소스로 사용하십시오.

Linux

. install/setup.bash

4. 실행

이제 static_turtle_tf2_broadcaster 노드를 실행하십시오.

ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

이렇게 하면 mystaticturtle 에 대한 거북이 포즈가 지면에서 1 미터 떨어진 위치로 설정됩니다.

이제 tf_static 토픽을 확인하여 정적 변환을 게시했는지 확인할 수 있습니다.

ros2 topic echo /tf_static

모든 것이 잘되었다면 단일 정적 변환을 볼 수 있어야합니다.

transforms:
- header:
   stamp:
      sec: 1622908754
      nanosec: 208515730
   frame_id: world
child_frame_id: mystaticturtle
transform:
   translation:
      x: 0.0
      y: 0.0
      z: 1.0
   rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

정적 변환을 게시하는 올바른 방법

이 튜토리얼은 StaticTransformBroadcaster 를 사용하여 정적 변환을 게시하는 방법을 보여주기 위한 것입니다. 실제 개발 프로세스에서는 직접 코드를 작성하지 않아도 되며 전용 tf2_ros 도구를 사용해야 합니다. tf2_ros 는 명령줄 도구 또는 런치 파일에 추가할 수 있는 노드로 사용할 수 있는 static_transform_publisher 라는 실행 가능 도구를 제공합니다.

미터 단위로 x/y/z 오프셋 및 라디안으로 roll/pitch/yaw를 사용하여 tf2에 정적 좌표 변환을 게시합니다. 우리의 경우 roll/pitch/yaw는 각각 x/y/z-축을 중심으로 회전하는 것을 의미합니다.

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --yaw yaw --pitch pitch --roll roll --frame-id frame_id --child-frame-id child_frame_id

미터 단위로 x/y/z 오프셋 및 쿼터니온을 사용하여 tf2에 정적 좌표 변환을 게시합니다.

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --qx qx --qy qy --qz qz --qw qw --frame-id frame_id --child-frame-id child_frame_id

static_transform_publisher 는 수동으로 사용할 명령줄 도구로 설계되었지만 정적 변환을 설정하기 위해 launch 파일 내에서도 사용할 수 있습니다. 예를 들어:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
             package='tf2_ros',
             executable='static_transform_publisher',
             arguments = ['--x', '0', '--y', '0', '--z', '1', '--yaw', '0', '--pitch', '0', '--roll', '0', '--frame-id', 'world', '--child-frame-id', 'mystaticturtle']
        ),
    ])

참고로 frame-id 및 child-frame-id 를 제외한 모든 인수는 선택 사항입니다. 특정 옵션이 지정되지 않으면 항등 변환을 가정합니다.

요약

이 튜토리얼에서는 StaticTransformBroadcaster 를 사용하여 정적 프레임 간의 정적 관계를 정의하는 데 유용한 방법을 배웠습니다. 예를 들어 world 프레임에 대한 mystaticturtle 와 같은 프레임 간의 관계를 정의하는 데 사용할 수 있습니다. 또한 정적 변환은 레이저 스캐너와 같은 센서 데이터를 이해하는 데 유용하며 데이터를 공통 좌표 프레임과 관련시키는 데 사용할 수 있습니다. 마지막으로 static_transform_publisher 실행 파일 및 런치 파일을 사용하여 필요한 정적 변환을 게시하는 방법을 배웠습니다.