新建一个tf发布者
//zq_broadcaster.cpp
#include <ros/ros.h> //编写ros程序所必须的
#include <tf/transform_broadcaster.h> //基于tf库
#include <turtlesim/Pose.h> //turtlesim功能包,乌龟位姿信息获取(机关它与世界坐标转换关系)
int main(int argc,char** argv)
{
//查看argc和argv,执行命令行提供的任何ROS參數和名称重映射
ros::init(argc,argv,"zq_broadcaster");
ros::NodeHandle node;
ros::Rate loop_rate(10);
while (ros::ok())
{
static tf::TransformBroadcaster br; //发送坐标转换
tf::Transform transform; //2D转换为3D
transform.setOrigin( tf::Vector3(2.0, 1.0, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, 1.5); //单位是弧度单位,正:逆时针旋转
transform.setRotation(q);
//transform;时间戳;父框架名称(world);子框架名称(turtle);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "turtle"));
ROS_INFO("pub world, turtle tf");
loop_rate.sleep();
}
return 0;
}
发布的坐标转换(在发布后最好用rviz查看是否正确)
订阅这个tf变换
//turtle_tf_listener.cpp
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
//#include <geometry_msgs/Twist.h> //将监听到的消息转换为速度消息
//#include <turtlesim/Spawn.h> //在同一窗口生成另一个乌龟turtle2(spawn再生服务)
int main(int argc, char** argv){
ros::init(argc, argv, "zq_listener");
ros::NodeHandle node;
//创建监听器,用于监听是否存在需要的tf
tf::TransformListener listener;
/*
*监听器查找特定的tf
*根据查找的tf信息计算相应的速度信息
*将速度信息发布给turtle2
*/
ros::Rate rate(10.0);
geometry_msgs::PointStamped world;
world.header.stamp=ros::Time();
world.header.frame_id="world";
world.point.x=1;
world.point.y=2;
world.point.z=3;
geometry_msgs::PointStamped turtle;
while (node.ok())
{
tf::StampedTransform transform;
try
{
/* 查找特定的tf
*"/turtle2", "/turtle1":从turtle2到turtle1的转换(frame name)
*ros::Time(0):指定tf时间,获取到最近存在的tf
*transform:保存查找到的tf
*/
listener.waitForTransform("/world", "/turtle", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/world", "/turtle",ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
ROS_INFO("x,y,z = %f,%f,%f",transform.getOrigin().x(),transform.getOrigin().y(),transform.getOrigin().z());
ROS_INFO("w,y = %f,%f",transform.getRotation().getW(),transform.getRotation().getY());
//transformPoint
try
{
listener.waitForTransform("/world", "/turtle", ros::Time(0), ros::Duration(3.0));
listener.transformPoint("/turtle",world,turtle);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
ROS_INFO("turtle.point.x=%f,turtle.point.y=%f,turtle.point.z = %f",turtle.point.x,turtle.point.y,turtle.point.z);
rate.sleep();
}
return 0;
};
输出结果
tf发布者输出
tf订阅者输出
注意事项
1 , tf监听时可监听两种坐标系的相互转化;
2 , listener.lookupTransform("/world", “/turtle”,ros::Time(0), transform);是指world坐标系里的坐标在,turtle坐标系里的表示;
3 , ros::Time(0):指定tf时间,获取到最近存在的tf
具体代码包详见:gitee-----learning_tf