출처 : http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF
tf란? tf를 왜 사용해야 하는가
- 위와 같이, robot system은 많은 3D coordinate frames를 가지고 있다.
- frame의 종류로는 world frame, base frame, gripper frame, head frame 등이 있고 이는 시간에 따라 계속 변화한다.
- tf는 위와 같은 frame 들을 계속 tracking하며 frame들 간의 상대적인 위치를 기록한다.
- tf는 distributed system에서 구동되기 때문에, 로봇의 coordinate frame 들에 대한 모든 정보는 다른 모든 ROS Component들도 사용할 수 있다.
- 즉, transform information은 중심 서버가 존재하지 않는다.
tf 환경 설정하기
Transform Configuration
- 추상적으로 설명하자면, transform tree는 서로 다른 coordinate frame들 사이의 회전과 평행 이동에 관한 offset들을 정의한다.
- 이를 좀더 상세히 설명하기 위해 아래와 같이 single laser가 장착된 로봇을 생각해보자.
- 이때 robot의 base의 중심점에 위치하는 frame과 single laser의 중심점에 위치한 frame, 총 두 개의 coordinate frame을 가정하고 robot의 base에 위치하는 frame을 "base_link", laser에 위치하는 frame을 "base_laser"라고 하자.
- 여기서 우리는 laser의 "중심점"으로부터 떨어진 거리를 laser로부터 얻을 수 있다. 즉, 우리는 "base_laser" coordinate frame의 정보를 가지고 있다.
- 이때 우리가 이 정보를 이용해 로봇이 장애물을 피하도록 하고 싶다면, 우리는 "base_laser" frame으로 부터 받은 정보를 "base_link" frame으로 변환시킬 방법이 필요하게 된다. 즉, 우리는 "base_laser"와 "base_link" frame간 관계를 정의해야 한다.
- 이 관계를 정의하기 위해 우리는 laser가 robot base의 중심점으로 부터 얼마나 떨어져 있는지를 알아야한다. 그리고 이것이 "base_link" frame과 "base_laser" frame간의 translational offset이 된다.
- translational offest을 통해 우리는 "base_link" frame과 "base_laser" frame간의 상호 정보 변환을 할 수 있게 된다.
- 이렇게 일일히 frame간의 관계를 직접 만들어 줄 수도 있지만, frame의 개수가 많아지면 쉬운일이 아니게 된다. 따라서 우리는 tf를 이용하여 tf가 우리 대신에 frame간의 정보 변환을 관리하도록 한다.
- tf를 이용해 "base_link"와 "base_laser" frame 사이의 관계를 정립하고 보관하려면, 우리는 그것들을 transform tree에 추가하여야 한다.
- transform tree의 각 node는 coordinate frame에 해당되고, 각각의 edge는 해당 node에서 그것의 child node로 이동할때 적용되어야하는 transform에 해당한다.
- Tf는 tree 구조를 통해 서로 다른 두 coordinate frame 사이의 관계를 일방통행으로 형성되도록 하였다. 따라서 tree의 모든 edge는 부모 노드로 부터 자식 노드로만 향하도록 해야 한다.
- 예를 통해 알아보자.
- "base_link" frame 노드 하나와 "base_laser" frame 노드 하나가 있다고 가정하면, 두 노드 상의 edge를 형성하기 위해 우리는 먼저 어느 node가 부모가 될 것인지를 정해야한다. tf는 부모로부터 자식으로의 transform만을 허용하기에 이 과정은 매우 중요하다.
- 이후 로봇에 다른 센서들이 추가로 부착되어 "base_laser" frame과 relate하게 된다면 "base_link"를 통해 가는것이 가장 합리적으로 판단되므로, "base_link"를 부모 노드로 하자.
- transform tree가 형성되고 나면, tf 라이브러리를 호출하기만하면 laser scan 정보를 쉽게 "base_link" frame으로 변환할 수 있게된다.
- 이 정보를 통해 로봇은 "base_link" frame 기준으로 laser scan에 대해 판단하고 장애물을 피할 계획을 세울수 있게 된다.
Writing Code
- 위에서는 예제를 통해 tf의 개념을 이해하였다면, 여기서는 transform tree를 code로 구현할 것이다.
- 위 예제와 같이 "base_laser" frame에서 좌표를 얻어 "base_link" frame으로 변환하는 작업을 수행해야 한다면, 첫번째로 우리가 해야 할 것은 로봇의 system에서 transform을 publish할 노드를 생성하는 것이다.
- 그 이후에 우리는 ROS에서 publish된 transform data를 listen 하는 노드를 생성하여 좌표를 변환하는데에 그를 이용할 것이다.
- 먼저 소스 코드가 자리할 package를 만들고, 그 이름을 "robot_setup_tf"로 하자. 이는 roscpp, tf, geometry_msgs에 dependency를 가질 것이다.
$ cd %TOP_DIR_YOUR_CATKIN_WS%/src
$ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs- 이때 위와 같은 command는 permission이 부여된 경로에서 수행하여야한다.
Broadcasting a Transform
- Package를 생성하고 나면, 이제 ROS에서 "base_laser -> base_link" transform을 broadcast할 노드를 생성하여야 한다.
- 생성한 package안에 아래의 코드를 복사하여 에디터를 통해 저장하자.
1 #include <ros/ros.h>
2 #include <tf/transform_broadcaster.h>
3
4 int main(int argc, char** argv){
5 ros::init(argc, argv, "robot_tf_publisher");
6 ros::NodeHandle n;
7
8 ros::Rate r(100);
9
10 tf::TransformBroadcaster broadcaster;
11
12 while(n.ok()){
13 broadcaster.sendTransform(
14 tf::StampedTransform(
15 tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
16 ros::Time::now(),"base_link", "base_laser"));
17 r.sleep();
18 }
19 }- 위 코드를 자세히 살펴보자.
#include <tf/transform_broadcaster.h>- tf package는 tf::TransformBroadcaster를 실행하여 transform을 publish하는 과정을 더 쉽게 해준다. 이 TransformBroadcaster를 사용하기 위해서 우리는 tf/transform_broadcaster.h 헤더파일을 include 해주어야한다.
tf::TransformBroadcaster broadcaster;- TransformBroadcaster 객체를 생성하였다. 이는 이후에 base_link -> base_laser transform을 수행할때에 이용할 것이다.
13 broadcaster.sendTransform(
14 tf::StampedTransform(
15 tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
16 ros::Time::now(),"base_link", "base_laser"));- 이 부분이 실제 transform이 이루어지는 부분이다.
- TransformBroadcaster를 통해 transform을 송신하려면 5개의 argument가 필요하다.
- 첫째로는 두개의 coordinate frame간 이루어져야 하는 rotation을 btQuaternion을 통해 명시한 rotation transform을 전달한다. 예제의 경우 rotation은 존재하지 않기에 btQuaternion의 pitch, roll, yaw에 해당하는 값은 0으로 설정하였다.
- 두번째로 btVector를 통해 우리가 적용하고 싶은 변환을 전달한다. 예제의 경우 robot base로 부터 x offset이 10cm, z offset이 20cm이므로 그에 해당하는 btVector3을 설정하였다.
- 셋째로 현재 publish되고있는 transform에 timestamp를 주어야한다. 이는 ros::Time::now()를 통해 이루어진다.
- 넷째로 부모 노드의 이름을 전달해 주어야한다. 예제의 경우 "base_link"가 될 것이다.
- 마지막으로 자식 노드의 이름을 전달해 주어야한다. 예제의 경우 "base_laser"가 될 것이다.
Using a Transform
- 위에서 우리는 base_laser -> base_link transform을 publish하는 노드를 생성하였다. 이제 우리는 "base_laser" frame의 한 점을 잡아 "base_link" frame의 한 점으로 변환해줄 노드를 만들 것이다.
- robot_setup_tf 패키지에 src/tf_listener.cpp라는 이름의 파일을 만들고, 아래 코드를 입력하자.
#include <ros/ros.h>
2 #include <geometry_msgs/PointStamped.h>
3 #include <tf/transform_listener.h>
4
5 void transformPoint(const tf::TransformListener& listener){
6 //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
7 geometry_msgs::PointStamped laser_point;
8 laser_point.header.frame_id = "base_laser";
9
10 //we'll just use the most recent transform available for our simple example
11 laser_point.header.stamp = ros::Time();
12
13 //just an arbitrary point in space
14 laser_point.point.x = 1.0;
15 laser_point.point.y = 0.2;
16 laser_point.point.z = 0.0;
17
18 try{
19 geometry_msgs::PointStamped base_point;
20 listener.transformPoint("base_link", laser_point, base_point);
21
22 ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
23 laser_point.point.x, laser_point.point.y, laser_point.point.z,
24 base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
25 }
26 catch(tf::TransformException& ex){
27 ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
28 }
29 }
30
31 int main(int argc, char** argv){
32 ros::init(argc, argv, "robot_tf_listener");
33 ros::NodeHandle n;
34
35 tf::TransformListener listener(ros::Duration(10));
36
37 //we'll transform a point once every second
38 ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));
39
40 ros::spin();
41
42 }- 마찬가지로 하나하나 살펴보자
#include <tf/transform_listener.h>- tf::TransformListener를 생성하기 위해 tf/transform_listenr.h 헤더 파일을 include 해야 한다. TransformListenr 객체는 자동으로 ROS 환경에 존재하는 transform message를 subscribe하고, 들어오는 모든 transform 데이터를 관리하는 역할을 수행한다.
void transformPoint(const tf::TransformListener& listener){- TransformListener라는 함수를 하나 생성하는데, 이는 "base_laser" frame의 한 점을 "base_link" frame의 점으로 변환해주는 역할을 한다.
- 이 함수는 system의 main() 함수에 있는 ros:Timer에 대한 callback으로서 작용하여, 매초마다 구동된다.
6 //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
7 geometry_msgs::PointStamped laser_point;
8 laser_point.header.frame_id = "base_laser";
9
10 //we'll just use the most recent transform available for our simple example
11 laser_point.header.stamp = ros::Time();
12
13 //just an arbitrary point in space
14 laser_point.point.x = 1.0;
15 laser_point.point.y = 0.2;
16 laser_point.point.z = 0.0;-
이 부분에서는 point를 geometry_msgs::PointStamped로 생성하였는데, 여기서 "Stamped"는 timestamp와 frame_id를 메세지에 연계시킬수있게 해주는 헤더를 이 객체가 include하고 있음을 의미한다.
-
이후 laser_point 메세지의 stamp field를 ros::Time()으로 설정한다. 이를 통해 TransformListener으로 부터 가장 최신의 transform을 얻을 수 있게 된다.
-
헤더의 frame_id field의 경우 현재 생성중인 point가 "base_laser" frame에 위치하므로 "base_laser"로 설정한다.
-
마지막으로 point에 값을 입력해 준다.
18 try{ 19 geometry_msgs::PointStamped base_point; 20 listener.transformPoint("base_link", laser_point, base_point); 21 22 ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", 23 laser_point.point.x, laser_point.point.y, laser_point.point.z, 24 base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec()); 25 } -
"base_laser" frame에 point를 설정하였으므로, 이제 그것을 "base_link" frame으로 변환하자.
-
이를 수행하기 위해 TransformListenr 객체를 이용하여 transformPoint()를 호출하자.
-
transformPoint()는 세개의 arguments를 가진다.
-> 기존의 point를 변환시킬 목적지 frame(예제의 경우 "base_link")
-> 변환할 point
-> 변환된 point를 보관할 공간 -
transformPoint()를 호출하면, base_point는 laser_point가 가지고 있던 정보와 동일한 정보를 가지게 된다. 여기서 차이점은 base_point는 "base_laser" frame이 아닌 "base_link" frame에 존재한다는 것이다.
26 catch(tf::TransformException& ex){ 27 ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what()); 28 } -
만약 transform이 정상적으로 수행되지 않으면 TransformListener가 exception을 throw하고, 오류메세지가 출력되도록 한다.
Building the Code
- roscreate-pkg에 의해 자동으로 생성된 CMakeLists.txt를 열고, 맨 아랫줄에 아래의 코드들을 입력하자.
add_executable(tf_broadcaster src/tf_broadcaster.cpp)
add_executable(tf_listener src/tf_listener.cpp)
target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})
target_link_libraries(tf_listener ${catkin_LIBRARIES})- 파일을 저장하고 package를 build하자.
$ cd %TOP_DIR_YOUR_CATKIN_WS%
$ catkin_makeRunning the Code
- 이제 ROS에서 어떻게 code가 수행되는지 살펴보자. 터미널창은 세 개가 필요할 것이다.
- 첫번째 터미널에서는 core를 구동시키자.
roscore- 두번째 터미널에서는 tf_broadcaster를 구동시키자.
rosrun robot_setup_tf tf_broadcaster- 마지막 터미널에서는 tf_listener를 구동시켜 "base_laser" frame의 point를 "base_link" frame으로 변환시키자.
rosrun robot_setup_tf tf_listener- 정상적으로 수행된다면, "base_laser" frame의 point가 1초마다 "base_link" frame의 point로 변환되는 것을 확인할 수 있을 것이다.
[ INFO] 1248138528.200823000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138528.19
[ INFO] 1248138529.200820000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138529.19
[ INFO] 1248138530.200821000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138530.19
[ INFO] 1248138531.200835000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138531.19
[ INFO] 1248138532.200849000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138532.19
초보 개발자