发布demo代码如下:
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
//初始化发布节点,demo_tf_broadcaster是节点的名字
ros::init(argc, argv, "demo_tf_broadcaster");
//创建一个tf发布对象
static tf::TransformBroadcaster br;
while(ros::ok())
{
//创建一个tf对象
tf::Transform transform;
//设置tf的位移量
transform.setOrigin( tf::Vector3(0.3369507, -0.37187648, -1.80606454));
//初始化一个四元数变量
tf::Quaternion q;
//设置俯仰角
q.setRPY(0.93040044, 0.43019939, -3.08609162);
//将四元数存入tf对象中,tf只能存储四元数。
transform.setRotation(q);
//"robot" 是父坐标,"world"是子坐标
//转换关系是从robot坐标系转到world坐标系
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "robot"));
ROS_INFO("The transform has broadcasted!");
ros::Duration(0.05).sleep();
}
return 0;
};
订阅demo代码如下:
#include <ros/ros.h>
#include <tf/transform_listener.h>
int main(int argc, char** argv){
ros::init(argc, argv, "demo_tf_listener"); //初始化一个监听器
ros::NodeHandle n; //创建一个句柄
tf::TransformListener listener; //声明一个监听器
float saved_rx = 0.0;
float saved_ry = 0.0;
float saved_rz = 0.0;
ros::Rate rate(10.0);
while (ros::ok()){
tf::StampedTransform transform;
try{
ros::Time now=ros::Time::now();
//订阅的是以robot作为父坐标系,world为子坐标系
listener.waitForTransform("world","robot",now,ros::Duration(1.0));
listener.lookupTransform("/world", "/robot",now, transform);
saved_rx = transform.getOrigin().x();
saved_ry = transform.getOrigin().y();
saved_rz = transform.getOrigin().z();
ROS_INFO("Transform rotation(RPY) received is %f %f %f",saved_rx,saved_ry,saved_rz);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
ROS_INFO("Saved transform is %f %f %f",saved_rx,saved_ry,saved_rz);
rate.sleep();
}
return 0;
}