Setting up your robot using tf

http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF

1. Transform Configuration

많은 ROS 패키지는 tf 소프트웨어 라이브러리를 사용하여 로봇의 변환 트리를 게시해야합니다. 추상 수준에서 변환 트리는 서로 다른 좌표 프레임 간의 변환 및 회전 측면에서 오프셋을 정의합니다.

좀 더 구체적으로 만들려면 단일 레이저가 장착 된 이동식베이스가있는 간단한 로봇의 예를 고려하십시오. 로봇을 참조하여 두 개의 좌표 프레임을 정의하겠습니다. 하나는 로봇베이스의 중심점에 해당하고 다른 하나는베이스의 상단에 장착 된 레이저의 중심점에 해당합니다. 또한 쉽게 참조 할 수 있도록 이름을 지정해 보겠습니다.

우리는 이동베이스에 부착 된 좌표 프레임을 "base_link"(탐색을 위해 로봇의 회전 중심에 배치해야 함)라고 부르고 레이저에 부착 된 좌표 프레임을 "base_laser"라고 부릅니다. 프레임 명명 규칙은 REP 105를 참조하십시오.

이 시점에서 레이저 중심점으로부터의 거리 형태의 레이저 데이터가 있다고 가정 해 봅시다. 다시 말해, "base_laser"좌표 프레임에 데이터가 있습니다. 이제 이 데이터를 가져 와서 모바일 베이스이 전 세계의 장애물을 피하는 데 도움이된다고 가정합니다. . 이를 성공적으로 수행하려면 "base_laser"프레임에서 "base_link"프레임으로 수신 한 레이저 스캔을 변환하는 방법이 필요합니다. 본질적으로 "base_laser"와 "base_link" 좌표 프레임 간의 관계를 정의해야합니다.

이 관계를 정의 할 때 레이저가 모바일베이스의 중심점보다 10cm 앞으로, 20cm 위에 장착되어 있다고 가정합니다. 이것은 "base_link"프레임과 "base_laser"프레임을 관련시키는 변환 오프셋을 제공합니다. 특히 "base_link"프레임에서 "base_laser"프레임으로 데이터를 가져 오려면 (x : 0.1m, y : 0.0m, z : 0.2m)의 변환을 적용하고 " base_laser "프레임에서"base_link "프레임으로 반대 변환 (x : -0.1m, y : 0.0m, z : -0.20m)을 적용해야합니다.

우리는이 관계를 스스로 관리하도록 선택할 수 있는데, 이는 필요할 때 프레임간에 적절한 변환을 저장하고 적용하는 것을 의미하지만, 좌표 프레임의 수가 증가함에 따라 이는 실제 고통이됩니다. 그러나 다행히도 이 작업을 직접 수행 할 필요는 없습니다. 대신 tf를 사용하여 "base_link"와 "base_laser"사이의 관계를 정의하고 두 좌표 프레임 사이의 변환을 관리하도록 하겠습니다.

tf를 사용하여 "base_link"와 "base_laser"프레임 간의 관계를 정의하고 저장하려면 변환 트리에 추가해야합니다. 개념적으로 변환 트리의 각 노드는 좌표 프레임에 해당하고 각 가장자리는 현재 노드에서 자식 노드로 이동하기 위해 적용해야하는 변환에 해당합니다. Tf는 트리 구조를 사용하여 두 좌표 프레임을 함께 연결하는 단일 순회 만 보장하고 트리의 모든 모서리가 부모에서 자식 노드로 향한다고 가정합니다.

간단한 예제를위한 변환 트리를 만들기 위해 "base_link" 좌표 프레임과 "base_laser" 좌표 프레임을 위한 두 개의 노드를 만듭니다.
그들 사이의 edge를 만들려면 먼저 부모가 될 노드와 자식이 될 노드를 결정해야합니다. tf는 모든 변환이 부모에서 자식으로 이동한다고 가정하기 때문에이 구분이 중요합니다.
다른 조각 / 센서가 로봇에 추가되면 "base_link" 프레임을 통과하여 "base_laser" 프레임과 관련되는 것이 가장 적합하기 때문에 "base_link" 좌표 프레임을 부모로 선택합시다.
이것은 "base_link"와 "base_laser"를 연결하는 에지와 관련된 변환이 (x : 0.1m, y : 0.0m, z : 0.2m)이어야 함을 의미합니다. 이 변환 트리가 설정되면 "base_laser" 프레임에서 수신 된 레이저 스캔을 "base_link" 프레임으로 변환하는 것은 tf 라이브러리를 호출하는 것만 큼 간단합니다.
당신의 로봇은이 정보를 사용하여 "base_link" 프레임에서 레이저 스캔에 대한 추론을하고 해당 환경의 장애물에 대한 안전한 계획을 할 수 있습니다.

2. Writing Code

"base_laser"프레임에서 포인트를 가져 와서 "base_link"프레임으로 변환하는 위에서 설명한 높은 수준의 작업이 있다고 가정합니다. 가장 먼저해야 할 일은 시스템에 변환을 게시 할 노드를 만드는 것입니다. 다음으로, ROS를 통해 게시 된 변환 데이터를 듣고 포인트를 변환하기 위해 적용 할 노드를 만들어야합니다. 소스 코드를위한 패키지를 만드는 것으로 시작하여 "robot_setup_tf"와 같은 간단한 이름을 부여합니다.

$ cd %TOP_DIR_YOUR_CATKIN_WS%/src
$ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

$ sudo apt-get install ros-%YOUR_ROS_DISTRO%-navigation-tutorials

3. transformPoint

이제 패키지를 얻었으므로 ROS를 통해 base_laser → base_link 변환을 브로드 캐스트하는 작업을 수행 할 노드를 작성해야합니다. 방금 생성 한 robot_setup_tf 패키지에서 즐겨 사용하는 편집기를 실행하고 다음 코드를 src / tf_broadcaster.cpp 파일에 붙여 넣습니다.

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

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");
  ros::NodeHandle n;

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;

  while(n.ok()){
    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));
    r.sleep();
  }
}

이제 base_link → base_laser 변환 게시와 관련된 코드를 자세히 살펴 보겠습니다.

#include <tf/transform_broadcaster.h>

tf 패키지는 변환 게시 작업을보다 쉽게 수행 할 수 있도록 tf :: TransformBroadcaster 구현을 제공합니다. TransformBroadcaster를 사용하려면 tf / transform_broadcaster.h 헤더 파일을 포함해야합니다.

tf::TransformBroadcaster broadcaster;

여기에서는 나중에 통신을 통해 base_link → base_laser 변환을 보내는 데 사용할 TransformBroadcaster 객체를 만듭니다.

    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));

실제 작업이 이루어지는 곳입니다. TransformBroadcaster를 사용하여 변환을 보내려면 5 개의 인수가 필요합니다.
먼저 두 좌표 프레임 사이에서 발생해야하는 회전에 대해 btQuaternion으로 지정된 회전 변환을 전달합니다. 이 경우 회전을 적용하지 않기 위해 피치, 롤 및 요 값으로 구성된 btQuaternion을 0으로 보냅니다.
둘째, 적용하려는 모든 번역에 대한 btVector3. 그러나 변환을 적용하려고하므로 로봇베이스에서 레이저의 x 오프셋 10cm 및 z 오프셋 20cm에 해당하는 btVector3을 만듭니다.
셋째, 타임 스탬프를 게시 할 변환을 제공해야합니다. ros :: Time :: now ()로 스탬프를 찍습니다.
넷째, 생성중인 링크의 상위 노드 이름 (이 경우 "base_link")을 전달해야합니다.
다섯째, 생성하는 링크의 하위 노드 이름 (이 경우 "base_laser")을 전달해야합니다.

4. Using a Transform

위에서 우리는 ROS를 통해 base_laser → base_link 변환을 게시하는 노드를 만들었습니다. 이제이 변환을 사용하여 "base_laser"프레임의 포인트를 가져 와서 "base_link"프레임의 포인트로 변환 할 노드를 작성하겠습니다. 다시 한 번 아래 코드를 파일에 붙여넣고 더 자세한 설명을 시작하겠습니다. robot_setup_tf 패키지에서 src / tf_listener.cpp라는 파일을 만들고 다음을 붙여 넣습니다.

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>

void transformPoint(const tf::TransformListener& listener){
  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  }
  catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }
}

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_listener");
  ros::NodeHandle n;

  tf::TransformListener listener(ros::Duration(10));

  //we'll transform a point once every second
  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

  ros::spin();

}

자 ... 이제 중요한 비트를 단계별로 살펴 보겠습니다.

#include <tf/transform_listener.h>

여기에 tf :: TransformListener를 생성하는 데 필요한 tf / transform_listener.h 헤더 파일이 포함됩니다. TransformListener 객체는 ROS를 통해 변환 메시지 주제를 자동으로 구독하고 유선을 통해 들어오는 모든 변환 데이터를 관리합니다.

void transformPoint(const tf::TransformListener& listener){

TransformListener가 주어지면 "base_laser"프레임에서 포인트를 가져 와서 "base_link"프레임으로 변환하는 함수를 작성합니다. 이 함수는 프로그램의 main ()에서 생성 된 ros :: Timer의 콜백 역할을하며 매 초마다 실행됩니다.

  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

여기에서 우리는 지점을 geometry_msgs :: PointStamped로 작성합니다. 메시지 이름의 끝에 "스탬프"는 헤더가 포함되어 있음을 의미하며, 타임 스탬프와 frame_id를 메시지와 연결할 수 있습니다. laser_point 메시지의 스탬프 필드를 ros :: Time ()으로 설정하여 TransformListener에 사용 가능한 최신 변환을 요청할 수있는 특수 시간 값입니다. 헤더의 frame_id 필드는 "base_laser"프레임에 점을 생성하기 때문에 "base_laser"로 설정합니다. 마지막으로 점에 대한 일부 데이터를 설정합니다. x : 1.0, y : 0.2 및 z : 0.0의 값을 선택합니다.

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  }

"base_laser"프레임에 포인트가 있으므로 "base_link"프레임으로 변환하려고합니다. 이를 위해 TransformListener 객체를 사용하고 점을 변환하려는 프레임의 이름 (이 경우 "base_link"), 변환 할 점, 변형 된 점에 대한 스토리지. 따라서 transformPoint ()를 호출 한 후 base_point는 "base_link"프레임에서 지금 만했던 laser_point와 동일한 정보를 보유합니다.

  catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }

어떤 이유로 base_laser → base_link 변환을 사용할 수없는 경우 (tf_broadcaster가 실행되고 있지 않은 경우) transformPoint ()를 호출 할 때 TransformListener에서 예외가 발생할 수 있습니다. 이를 올바르게 처리하기 위해 예외를 포착하고 사용자에 대한 오류를 인쇄합니다.

5. 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})

Next, make sure to save the file and build the package.

6. Running the Code

rosrun robot_setup_tf tf_broadcaster

rosrun robot_setup_tf tf_listener