프레임 추가 (C++)

목표: tf2에 추가적인 프레임을 추가하는 방법을 배웁니다.

튜토리얼 레벨: 중급

소요 시간: 15분

배경

이전 튜토리얼에서는 tf2 브로드캐스터 및 tf2 리스너 를 작성하여 거북이 데모를 재생성했습니다. 이 튜토리얼에서는 변환 트리에 추가로 고정 및 동적 프레임을 추가하는 방법을 가르쳐 드립니다. 실제로 tf2에 프레임을 추가하는 것은 tf2 브로드캐스터를 만드는 것과 매우 유사하지만 이 예제에서는 tf2의 몇 가지 추가 기능을 보여줍니다.

변환 작업과 관련된 많은 작업에서는 지역 프레임 내에서 생각하는 것이 더 쉽습니다. 예를 들어, 레이저 스캔 측정 값을 레이저 스캐너 중심의 프레임에서 생각하는 것이 가장 쉽습니다. tf2를 사용하면 시스템 내의 각 센서, 링크 또는 조인트마다 지역 프레임을 정의할 수 있습니다. 한 프레임에서 다른 프레임으로 변환할 때 tf2는 도입되는 모든 중간 프레임 변환을 처리합니다.

tf2 트리

tf2는 프레임의 트리 구조를 작성하며, 프레임 구조에 닫힌 루프를 허용하지 않습니다. 이것은 프레임이 하나의 단일 부모만 가지지만 여러 개의 자식을 가질 수 있음을 의미합니다. 현재 우리의 tf2 트리에는 world, turtle1, turtle2 라는 세 개의 프레임이 있습니다. 두 개의 turtle 프레임은 world 프레임의 자식들입니다 새로운 프레임을 tf2에 추가하려면 기존의 세 개 프레임 중 하나가 부모 프레임이어야하며, 새로운 프레임이 그의 자식 프레임이 될 것입니다.

작업

1 고정 프레임 브로드캐스터 작성

거북이 예제에서 새 프레임인 carrot1 을 추가할 것입니다. 이 프레임은 turtle1 의 자식이 될 것입니다. 이 프레임은 두 번째 거북이의 목표 역할을 합니다.

먼저 소스 파일을 생성합시다. 이전 튜토리얼에서 만든 learning_tf2_cpp 패키지로 이동하십시오. src 디렉토리에서 다음 명령을 입력하여 고정 프레임 브로드캐스터 코드를 다운로드합니다.

Linux

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

이제 fixed_frame_tf2_broadcaster.cpp 라는 파일을 엽니다.

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

class FixedFrameBroadcaster : public rclcpp::Node
{
public:
  FixedFrameBroadcaster()
  : Node("fixed_frame_tf2_broadcaster")
  {
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
      100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this));
  }

private:
  void broadcast_timer_callback()
  {
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 0.0;
    t.transform.translation.y = 2.0;
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
  }

rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

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

이 코드는 tf2 브로드캐스터 튜토리얼 예제와 매우 유사하지만 여기서는 변환 값이 시간이 지남에 따라 변경되지 않습니다.

1.1 코드 검토

이 코드에서 주요 라인을 살펴보겠습니다. 우리는 여기서 새로운 변환을 생성하며 부모 turtle1 에서 자식 carrot1 로 변환합니다. carrot1 프레임은 turtle1 프레임과 비교하여 y 축에서 2 미터 떨어진 위치에 있습니다.

geometry_msgs::msg::TransformStamped t;

t.header.stamp = this->get_clock()->now();
t.header.frame_id = "turtle1";
t.child_frame_id = "carrot1";
t.transform.translation.x = 0.0;
t.transform.translation.y = 2.0;
t.transform.translation.z = 0.0;

1.2 CMakeLists.txt

learning_tf2_cpp 디렉토리로 이동하십시오. 여기에 CMakeLists.txt 및 package.xml 파일이 위치합니다.

이제 CMakeLists.txt 파일을 열고 실행 파일을 추가하고 fixed_frame_tf2_broadcaster 라고 이름을 지정하십시오.

add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
    fixed_frame_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2_ros
)

마지막으로 실행 파일을 찾을 수 있도록 install(TARGETS…) 섹션을 추가하십시오. 이렇게 하면 ros2 run 명령이 실행 파일을 찾을 수 있습니다.

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

1.3 런치 파일 작성

이제 이 예제를 위한 런치 파일을 만들어 봅시다. 텍스트 편집기를 사용하여 launch/turtle_tf2_fixed_frame_demo.launch.py 라는 새 파일을 만들고 다음 라인을 추가합니다.

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_cpp'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
        )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_cpp',
            executable='fixed_frame_tf2_broadcaster',
            name='fixed_broadcaster',
        ),
    ])

이 런치 파일은 필요한 패키지를 가져오고 이전 튜토리얼의 런치 파일에서 생성한 노드를 저장할 demo_nodes 변수를 만듭니다.

코드의 마지막 부분은 고정 carrot1 프레임을 fixed_frame_tf2_broadcaster 노드를 사용하여 turtlesim 월드에 추가합니다.

Node(
    package='learning_tf2_cpp',
    executable='fixed_frame_tf2_broadcaster',
    name='fixed_broadcaster',
),

1.4 빌드

작업 공간의 루트에서 빠진 종속성을 확인하려면 rosdep 를 실행하십시오.

Linux

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

작업 공간의 루트에서 패키지를 빌드하십시오.

Linux

colcon build --packages-select learning_tf2_cpp

새 터미널을 열고 작업 공간의 루트로 이동하고 설정 파일을 소스로합니다.

Linux

. install/setup.bash

1.5 실행

이제 거북이 브로드캐스터 데모를 시작할 수 있습니다.

ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py

새로운 carrot1 프레임이 변환 트리에 나타난 것을 볼 수 있어야합니다.

첫 번째 거북이를 주위로 움직이면 새 프레임을 추가했지만 동작이 이전 튜토리얼과 변경되지 않았음을 알 수 있습니다. 이것은 추가 프레임이 다른 프레임에 영향을 미치지 않으며 리스너가 이전에 정의된 프레임을 사용하기 때문입니다.

따라서 두 번째 거북이가 첫 번째 turtle가 아닌 carrot을 따라가기를 원한다면 target_frame 의 값을 변경해야합니다. 이것은 두 가지 방법으로 수행 할 수 있습니다. 하나는 콘솔에서 런치 파일로 target_frame 인수를 직접 전달하는 것입니다.

ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1

두 번째 방법은 런치 파일을 업데이트하는 것입니다. 이를 위해 turtle_tf2_fixed_frame_demo.launch.py 파일을 열고 launch_arguments 인수를 통해 'target_frame': 'carrot1' 매개 변수를 추가하십시오.

def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        ...,
        launch_arguments={'target_frame': 'carrot1'}.items(),
        )

이제 패키지를 다시 빌드하고 turtle_tf2_fixed_frame_demo.launch.py 를 다시 시작하면 첫 번째 turtle 대신 두 번째 turtle가 carrot을 따라가는 것을 볼 수 있습니다!

2 동적 프레임 브로드캐스터 작성

이 튜토리얼에서 추가 한 추가 프레임은 부모 프레임에 대해 시간에 따라 변경되지 않는 고정 프레임입니다. 그러나 움직이는 프레임을 게시하려면 브로드캐스터를 코드화하여 프레임을 시간에 따라 변경하도록 할 수 있습니다. 이제 carrot1 프레임을 turtle1 프레임에 대해 시간에 따라 변경되도록 변경해 보겠습니다. learning_tf2_cpp 패키지에 이전 튜토리얼에서 생성한 동적 프레임 브로드캐스터 코드를 다운로드하려면 src 디렉토리로 이동하고 다음 명령을 입력하십시오.

Linux

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

이름이 dynamic_frame_tf2_broadcaster.cpp 인 파일을 엽니다.

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

const double PI = 3.141592653589793238463;

class DynamicFrameBroadcaster : public rclcpp::Node
{
public:
  DynamicFrameBroadcaster()
  : Node("dynamic_frame_tf2_broadcaster")
  {
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
      100ms, std::bind(&DynamicFrameBroadcaster::broadcast_timer_callback, this));
  }

private:
  void broadcast_timer_callback()
  {
    rclcpp::Time now = this->get_clock()->now();
    double x = now.seconds() * PI;

    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = now;
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 10 * sin(x);
    t.transform.translation.y = 10 * cos(x);
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
  }

  rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

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

2.1 코드 검토

현재 시간에 sin() 및 cos() 함수를 사용하여 carrot1 의 오프셋이 지속적으로 변경되도록하므로 x 및 y 오프셋의 고정 정의 대신에 현재 시간을 사용합니다.

double x = now.seconds() * PI;
...
t.transform.translation.x = 10 * sin(x);
t.transform.translation.y = 10 * cos(x);

2.2 CMakeLists.txt

learning_tf2_cpp 디렉토리로 이동하십시오. 여기에 CMakeLists.txt 및 package.xml 파일이 위치합니다.

이제 CMakeLists.txt 파일을 열고 실행 파일을 추가하고 dynamic_frame_tf2_broadcaster 라고 이름을 지정하십시오.

add_executable(dynamic_frame_tf2_broadcaster src/dynamic_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
    dynamic_frame_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2_ros
)

마지막으로 실행 파일을 찾을 수 있도록 install(TARGETS…) 섹션을 추가하십시오. 이렇게 하면 ros2 run 명령이 실행 파일을 찾을 수 있습니다.

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

2.3 런치 파일 작성

이제 이 예제를 위한 런치 파일을 만들어 봅시다. 텍스트 편집기로 새 파일을 만들고 launch/turtle_tf2_dynamic_frame_demo.launch.py 라고 명명한 다음 다음 코드를 추가하십시오.

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_cpp'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
        )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_cpp',
            executable='dynamic_frame_tf2_broadcaster',
            name='dynamic_broadcaster',
        ),
    ])

2.4 빌드

작업 공간의 루트에서 빠진 종속성을 확인하려면 rosdep 를 실행하십시오.

Linux

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

작업 공간의 루트에서 패키지를 다시 빌드하십시오.

Linux

colcon build --packages-select learning_tf2_cpp

새 터미널을 열고 작업 공간의 루트로 이동하고 설정 파일을 소스합니다.

Linux

. install/setup.bash

2.5 실행

이제 동적 프레임 데모를 시작할 수 있습니다:

ros2 launch learning_tf2_cpp turtle_tf2_dynamic_frame_demo.launch.py

두 번째 거북이가 지속적으로 변하는 carrot의 위치를 따르는 것을 볼 수 있어야합니다.

요약

이 튜토리얼에서는 tf2 변환 트리, 그 구조 및 기능에 대해 배웠습니다. 또한 지역 프레임 내에서 생각하는 것이 가장 쉽다는 것을 배우고 지역 프레임에 대한 추가 고정 및 동적 프레임을 추가하는 방법을 배웠습니다.