ros的tf坐标demo

#include<ros/ros.h>

#include<tf/transform_broadcaster.h>

#include<turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::Pose::ConstPtr& msg)

{

static tf::TransformBroadcaster br;//创建一个tf广播器

//根据乌龟当前的位姿，设置相对于世界坐标系的坐标变换

tf::Transform transform;

transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));//根据turtle/pose设置转换元素

tf::Quaternion q;//创建一个四元数

q.setRPY(0,0,msg->theta);//使用固定轴RPY设置四元数

transform.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];

//订阅乌龟的pose信息

ros::NodeHandle node;

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

ros::spin();

return 0;

}