turtle_tf_broadcaster.cpp

/* 로봇의 pose 좌표를 받아서 tf로 변환하는 예제 */

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

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr &msg) {
    static tf::TransformBroadcaster br;
    tf::Transform transform;

    // xyz 좌표 변환
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    tf::Quaternion q;
    // 각도에 대해서는 쿼터니언으로 변환
    q.setRPY(0, 0, msg->theta);
    transfrom.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char **argv) {
    // 노드 초기화
    ros::init(argc, argv, "my_tf_broadcaster");
    if(argc != 2) {
        ROS_ERROR("need turtle name as argument");
        return -1;
    }
    turtle_name = argv[1];

    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &posecCallback);

    ros::spin();
    return 0;
}