ROS 연구 노트 : TF 좌표 변환 및 세부 프로그래밍

1. 소개 및 TF 소개

TF 란?

tf는 사용자가 시간에 따라 여러 참조 시스템을 추적 할 수 있도록하는 함수 패키지입니다. 트리 형 데이터 구조를 사용하여 시간에 따라 여러 참조 시스템 간의 좌표 변환 관계를 버퍼링하고 유지 관리합니다. 사용자가 언제든지 포인트를 설정할 수 있도록 도와줍니다. 벡터 등의 데이터 좌표는 두 개의 참조 시스템에서 좌표 변환을 완료합니다.

간단히 말해서 로봇이 작업하는 동안 로봇 자체가 세계 좌표계에 있고 동시에 로컬 좌표계가 있다는 것을 알고 있습니다.이 두 좌표계의 존재는 로봇의 포즈를 계산할 수 있음을 보장합니다.

하지만 로봇 바디의 경우 포즈에서 결정해야 할 부분이 많고 시간에 따라 상태가 다를 수 있습니다. 하단 프레임을베이스 마크로 사용하면 차이를 어떻게 결정합니까? 라이다, 그리퍼, 관절, 머리와 같은 로봇 내부의 위치와 자세는 어떻습니까? 가장 좋은 방법은 이러한 것들에 각각의 좌표계를 설정하고 좌표 변환을 통해 포즈를 취하는 것입니다.

TF의 기능은 모든 좌표계 사이에서 시스템의 모든 지점의 좌표 변환을 실현하는 데 도움이되는 것입니다. 시스템의 특정 지점에 대해 TF는 다른 좌표계에서 지점의 좌표를 변환하는 데 도움을 줄 수 있습니다.
여기에 사진 설명 삽입

2. 자세한 TF 프로그래밍 예제

그렇다면 TF를 어떻게 적용할까요? 먼저 ROS에서 제공하는 예제를 데모로 보겠습니다.

공식 TF 튜토리얼

먼저 TF 기능 팩을 설치합니다.

sudo apt-get install ros-melodic-turtle-tf

다음 명령을 실행하십시오.

roslaunch turtle_tf turtle_tf_demo.launch
rosrun turtlesim turtle_teleop_key

키보드 컨트롤을 통해 거북이가 움직 이도록 초기화하면 다른 거북이가 자동으로 따라 오는 것을 볼 수 있습니다.
여기에 사진 설명 삽입
이 프로세스는 주로 turtle1과 turtle2 간의 좌표 변환에 의해 실현됩니다. 우리는 통과 할 수 있습니다

rosrun tf view_frames	//创建一个TF监听器监听5s，得到TF结构关系图
rqt_graph				//查看节点信息

시스템에서 실행되는 모든 좌표계의 관계 구조 다이어그램 및 노드 정보 다이어그램을 가져옵니다.
여기에 사진 설명 삽입

2.1 공통 데이터 유형

ROS Wiki에 제공된 TF 데이터 유형을 참조하십시오. 주로 쿼터니언, 벡터, 점 좌표, 포즈 및 변환 템플릿에 해당하는 다음과 같은 기본 유형이 있습니다.
여기에 사진 설명 삽입
또한 tf :: Stamped 도 포함되어 있습니다. 공식 소스 코드는 다음과 같습니다. 즉,이 데이터 유형은 위의 모든 기본 유형 (tf :: Transform 제외)을 기반으로하여 frame_id_ 및 stamp_ 요소로 템플릿 화됩니다.

이 또한 TF :: StampedTransform TF의 특별한 경우이다 :: 변환 및 frame_id, 스탬프 및 child_frame_id이 필요합니다.
여기에 사진 설명 삽입
총 6 개의 데이터 유형.

2.2 TF 사용 방법

여기에 사진 설명 삽입
TF 패키지를 사용할 때 TF 변환 모니터링 및 TF 변환 방송 기능을 수행하기 위해 두 개의 프로그램을 작성해야하는데,이를 TF 리스너와 TF 브로드 캐스터라고 부릅니다.

TF 모니터 : TF 변환을 모니터링하고 시스템에 게시 된 모든 참조 시스템 변환을 수신 및 캐시하며 여기에서 필요한 참조 시스템 변환을 쿼리합니다.
TF 브로드 캐스터 : 방송 TF 변환, 시스템의 참조 시스템 간의 좌표 변환 관계를 방송합니다. 시스템에 다른 부분의 여러 tf 변환 브로드 캐스트가있을 수 있지만 각 브로드 캐스트는 동기화없이 참조 프레임의 변환 관계를 tf 트리에 직접 삽입 할 수 있습니다.

공식 튜토리얼을 예로 들어 보겠습니다. 프로그램은 위에서 주어진 거북이 다음과 같은 효과를 얻습니다.

2.3 TF 방송사

실현 기능 : TF 브로드 캐스터 생성, 좌표 변환 값 생성 및 실시간 좌표 변환 게시

프로그래밍 아이디어 :

ROS 노드를 초기화하고 거북이 위치 정보를 구독합니다.
루프에서 토픽 메시지를 기다렸다가 수신 후 콜백 함수를 입력하면 콜백 함수는 좌표 변환을 처리하고 게시하는 데 사용됩니다.
콜백 함수 내에 브로드 캐스터를 정의하십시오.
받은 작은 거북이의 위치 메시지를 기반으로 좌표 변환 값을 만듭니다.
정의 된 방송사를 통해 좌표 변환 게시

다음은 특정 구현의 코드 부분입니다.

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

std::string turtle_name;

void PoseCallBack(const turtlesim::PoseConstPtr& msg)
{
    
    
	//定义一个tf广播器
	static tf::TransformBroadcaster br;
	
	//根据turtle的位置信息，得到其相对于世界坐标系的变换
	tf::Transform transform;
	transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
	tf::Quaternion q;
	q.setRPY(0,0,msg->theta);
	transform.setRotation(q);
	
	//tf广播器发布2只海龟相对于世界坐标系的坐标变换
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    
    
	//初始化节点
	ros::init(argc, argv, "my_tf_broadcaster");
	
	//判断并且获取turtle_name
	if(argc != 2)
	{
    
    
		ROS_ERROR("Need a turtle name!");
		return -1;
	}
	turtle_name = argv[1];
	
	//订阅turtle的pose信息
	ros::NodeHandle n;
	ros::Subscriber sub = n.subscribe(turtle_name + "/pose", 10, &PoseCallBack);
	
	ros::spin();
	
	return 0;
}

코드 설명 :

여기서 tf 패키지를 배운 학생들은 ROS 내부 메시지의 게시 및 구독에 익숙 할 것입니다. 여기서는 콜백 함수에서 tf 변환과 관련된 코드 내용에 대해 주로 이야기하겠습니다. C ++에 익숙하지 않은 일부 학생들의 경우 argc 및 argv의 의미에 대한 자세한 내용은 여기를 참조하십시오 . 주 함수의 argc 및 argv

구체적인 분석에 앞서 "transform_broadcaster.h"의 내용을 살펴 보겠습니다.

#ifndef TF_TRANSFORMBROADCASTER_H
#define TF_TRANSFORMBROADCASTER_H
 
#include "tf/tf.h"
#include "tf/tfMessage.h"
 
#include <tf2_ros/transform_broadcaster.h>
 
namespace tf
{
    
    
class TransformBroadcaster{
    
    
public:
  TransformBroadcaster();
 
  void sendTransform(const StampedTransform & transform);
 
  void sendTransform(const std::vector<StampedTransform> & transforms);
 
  void sendTransform(const geometry_msgs::TransformStamped & transform);
 
  void sendTransform(const std::vector<geometry_msgs::TransformStamped> & transforms);
 
private:
 
  tf2_ros::TransformBroadcaster tf2_broadcaster_;
 
};
 
}
 
#endif //TF_TRANSFORMBROADCASTER_H

TransformBroadcaster 클래스는 네임 스페이스 tf 내에 정의되어 있습니다.이 클래스의 내부 내용도 매우 간단합니다.

매개 변수없는 생성자를 선언했습니다. TransformBroadcaster ();
함수 오버로딩을 사용하는 방법은 동일한 이름을 가진 여러 함수 sendTransform을 정의합니다.
민영화 된 멤버 변수 tf2_broadcaster_가 선언되었습니다.

이 내용을 기반으로 콜백 함수 코드의 수준을 다음과 같이 나눕니다.

(1) tf 브로드 캐스터 정의

static tf::TransformBroadcaster br;

tf :: TransformBroadcaster에는 인수가없는 생성자가 있으므로 초기화 중에 기본 생성자가 직접 호출되어 tf 브로드 캐스터 br을 선언합니다.

(2) 좌표 변환 생성

tf 내부의 데이터 유형에 따라 먼저 "Transform"데이터 구조를 선언하여 변환 내용을 기록합니다.

tf::Transform transform;

이 내용을 위해 우리는 변형 된 자세, 즉 위치와 방향을 결정해야합니다. Transform 클래스에서 함수를 찾습니다 : http://docs.ros.org/melodic/api/tf/html/c++/classtf_1_1Transform.html

다음 두 가지 기능은 우리의 요구를 충족시킬 수 있습니다. 내부 매개 변수는
여기에 사진 설명 삽입
추가 검색을 위해 "Vector3"및 "Quaternion"이어야합니다 (관심이있는 경우 공식 웹 사이트로 이동하여 여기에서 반복하지 않습니다). Vector3 유형은 선언하여 직접 사용할 수 있으며 Quaternion 유형은 먼저 setRPY를 사용해야합니다. 이 기능은 할당을 수행합니다. RPY과 관련, 참조하십시오 UAV의 요 각도, 롤 각도, 피치 각도 설명.
여기에 사진 설명 삽입
이로부터 우리가 얻을 수 있습니다

transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
transform.setRotation(tf::Quaternion::setRPY(0, 0, msg->theta));

얻기 위해 더 단순화

tf::Transform transform;
transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
tf::Quaternion q;
q.setRPY(0,0,msg->theta);
transform.setRotation(q);

(3) 방송사가 좌표 변환을 게시합니다.

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

앞서 설명한 "transform_broadcaster.h"의 내용에 따르면
여기에 사진 설명 삽입
내부 매개 변수 유형이 "StampedTransform" 이라는 것을 이해하기 어렵지 않을 것 입니다. 다음 그림은 상속 다이어그램을 보여 주며 이는 StampedTransform 클래스의 내용이 Transform에서 상속됨을 의미합니다.
tf :: StampedTransform의 상속 다이어그램
입력 (예 : 필요한 좌표 변환), 타임 스탬프, 프레임 ID 및 하위 프레임 ID를 사용할 때 내부적으로 선언해야합니다.
여기에 사진 설명 삽입

2.4 TF 리스너

실현 기능 : TF 모니터 생성, 두 번째 거북이 생성, 좌표 변환 모니터링 및 동작 제어 명령을 발행하여 두 번째 거북이가 첫 번째 거북이로 이동하도록합니다.

프로그래밍 아이디어 :

ROS 노드를 초기화하고 MASTER에 노드 정보를 등록합니다.
서비스 호출을 통해 두 번째 거북이를 생성합니다.
turtle2에 대한 속도 제어 게시자를 만듭니다.
tf 리스너를 만들고 turtle1에 대한 turtle2의 좌표 변환을 모니터링합니다.
좌표 변환에 따라 속도 제어 명령을 내립니다.

다음은 특정 구현의 코드 부분입니다.

#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 n;
	
	//service调用产生第二只海龟
	ros::service::waitForService("spawn");
	ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);
	
	//定义速度控制指令的消息发布者
	ros::Publisher turtle_vel = n.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
	
	//定义一个tf监听器
	tf::TransformListener listener;
	
	ros::Rate rate(10.0);
	
	while(ros::ok())
	{
    
    
		//申明一个空的坐标变换
		tf::StampedTransform transform;
		try
		{
    
    
			//监听turtle2和turtle1的坐标变换
			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
		}
		catch(tf::TransformException &ex)
		{
    
    
			ROS_ERROR("%s", ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
		//根据坐标变换计算得出turtle2的角速度和线速度，并发布该消息
		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;
}

여기서는 주로 tf 부분에 대해 이야기하고 "transform_listener.h"의 내용은 너무 많아서 여기에 넣지 않겠습니다 ~ 공식 문서는 직접 확인하실 수 있습니다.

반대로 위키에서 리스너를 사용하는 방법을 살펴볼 수도 있습니다 : http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms

(1) tf 리스너 정의

먼저 TransformListener의 내용이 Transformer 클래스에서 상속된다는 점을 명확히해야하므로 (변형 클래스가 아님에 유의) 사용시 동시에 두 문서를 확인해야합니다. 물론 가장 쉬운 방법은 위키에서 listener 사용을 보는 것입니다.
여기에 사진 설명 삽입
내부 구조 및 소멸자는 다음과 같습니다 (헤더 파일에서 가져옴) :

대부분의 경우 다음 명령을 사용하여

tf::TransformListener listener

(2) 좌표 변환 모니터링

먼저 빈 변환을 선언하십시오.

tf::StampedTransform transform;

그렇다면 tf :: Transform 대신 tf :: StampedTransform을 사용하여 여기서 선언하는 이유는 무엇입니까? 메시지를 게시하기 위해 tf 브로드 캐스터가 사용하는 데이터 유형에 대해 신중하게 생각해 봅시다.

시험

listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);

Wiki는 다음과 같이 설명합니다.

① tf :: TransformListener :: waitForTransform 함수는 source_frame을 시간에 target_frame으로 변환 할 수 있는지 여부를 테스트하고 변환이 수행 될 수 있는지 여부를 나타내는 bool 유형 값을 반환합니다.
여기에 사진 설명 삽입
② tf :: TransformListener :: lookupTransform 함수는 두 좌표계 간의 변환을 반환하고 반환 된 변환 방향은 target_frame에서 source_frame으로입니다.

(3) 문제가 발생하면 어떻게해야합니까?

모든 tf 예외 클래스는 std :: runtime_error에서 상속되는 tf :: TransformException에서 상속됩니다.
여기에 사진 설명 삽입

그러면 다음 코드의 내용은 what ()을 사용하여 오류 메시지가 잡힐 때 오류의 기본 정보를 추출하고 오류 메시지를 인쇄 한 다음 시스템이 일정 시간 동안 멈춘 다음 루프를 계속하는 것입니다.

ROS_ERROR("%s", ex.what());
ros::Duration(1.0).sleep();
continue;

3 효과 달성

프로그램을 작성한 후 CMakeLists
여기에 사진 설명 삽입
파일에 Launch 파일 을 추가 하고 구성합니다.

<launch>
	<node pkg="turtlesim" type="turtlesim_node" name="sim"/>

	<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen" />

	<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
	<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />

	<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />

</launch>

커맨드 라인에서 런치 파일을 실행 한 후 효과도는 다음과 같습니다.
여기에 사진 설명 삽입
이 시점에서 rqt_graph를 사용하여 노드 정보를보고 기본적으로 tf 함수 패키지를 사용할 때와 동일 함을 확인합니다 . 이 시점에서 tf.js

의 기본 프로그래밍 학습 내용을 완료했습니다.

참고

ROS에서 TF의 기본 역할 및
ROS 탐색 요약에 대한 간략한 소개 (18)-다시 읽기 tf
http://wiki.ros.org/tf
http://docs.ros.org/jade/api/tf/html/c++/ namespacetf.html
http://wiki.ros.org/roscpp/Overview/Time
ROS-TF 라이브러리 -TF 및 ros :: Time (0)
ROS 로봇의 tf 변환

재 인쇄를위한 소스 정보를 표시하십시오.