ROS发布tf坐标
我们写个小程序来发布一个坐标系：
坐标系消息格式：
std_msgs/Header header #头信息
uint32 seq #|-- 序列号
time stamp #|-- 时间戳
string frame_id #|-- 坐标 ID
string child_frame_id #子坐标系的 id
geometry_msgs/Transform transform #坐标信息
geometry_msgs/Vector3 translation #偏移量
float64 x #|-- X 方向的偏移量
float64 y #|-- Y 方向的偏移量
float64 z #|-- Z 方向上的偏移量
geometry_msgs/Quaternion rotation #四元数
float64 x
float64 y
float64 z
float64 w用下面的代码发布出来：
#include "ros/ros.h"
#include
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "ik_test_node");
ros::NodeHandle n;
tf2_ros::StaticTransformBroadcaster broadcaster;
geometry_msgs::TransformStamped ts;
ts.header.seq=100;
ts.header.stamp=ros::Time::now();
ts.header.frame_id="base_link";
ts.child_frame_id="laser";
ts.transform.translation.x=0.0;
ts.transform.translation.y=0.0;
ts.transform.translation.z=0.0;
ts.transform.rotation.x=0;
ts.transform.rotation.y=0;
ts.transform.rotation.z=0;
ROS_INFO("nihao woshi :%lf",ts.transform.rotation.x);
tf2::Quaternion qtn;
qtn.setRPY(0,0,0);
ts.transform.rotation.x=qtn.getX();
ts.transform.rotation.y=qtn.getY();
ts.transform.rotation.z=qtn.getZ();
ts.transform.rotation.w=qtn.getW();
broadcaster.sendTransform(ts);
ros::spin();
} 运行程序，然后打开rviz载入坐标系，就可以看到我们发布的坐标系了：
如下图：
发布一个动态的laser坐标系：
#include "ros/ros.h"
#include
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "ik_test_node");
ros::NodeHandle n;
tf2_ros::StaticTransformBroadcaster broadcaster;
geometry_msgs::TransformStamped ts;
ts.header.seq=100;
ts.header.stamp=ros::Time::now();
ts.header.frame_id="base_link";
ts.child_frame_id="laser";
ts.transform.translation.x=0.2;
ts.transform.translation.y=0.0;
ts.transform.translation.z=0.5;
ts.transform.rotation.x=0;
ts.transform.rotation.y=0;
ts.transform.rotation.z=0;
ROS_INFO("nihao woshi :%lf",ts.transform.rotation.x);
tf2::Quaternion qtn;
qtn.setRPY(0,0,0);
ts.transform.rotation.x=qtn.getX();
ts.transform.rotation.y=qtn.getY();
ts.transform.rotation.z=qtn.getZ();
ts.transform.rotation.w=qtn.getW();
ros::Rate r(1);
while(ros::ok())
{
ts.transform.translation.x+=0.2;
broadcaster.sendTransform(ts);
r.sleep();
}
broadcaster.sendTransform(ts);
ros::spin();
} 订阅方将点的坐标从laser坐标系转换到base_link坐标系：
#include
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
int main(int argc,char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"tf_sub");
ros::NodeHandle nh;
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate r(1);
while(ros::ok())
{
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id="laser";
point_laser.header.stamp=ros::Time::now();
point_laser.point.x=1;
point_laser.point.y=2;
point_laser.point.z=7.3;
try
{
geometry_msgs::PointStamped point_base;
point_base=buffer.transform(point_laser,"base_link");
ROS_INFO("zhuanhuanhoude shuju x:%f,y:%f,z:%f",point_base.point.x,point_base.point.y,point_base.point.z);
}
catch(const std::exception& e)
{
ROS_INFO("chengxu yichang");
}
r.sleep();
ros::spinOnce();
}
}
打开rviz，加载坐标系，就可以看到这两个坐标系了，这次这个laser坐标系逐渐远离base_link。
动态坐标转换：
订阅小乌龟位置和方向，然后发布出来。
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
void doPose(const turtlesim::Pose::ConstPtr& pose)
{
static tf2_ros::TransformBroadcaster broadcaster;
geometry_msgs::TransformStamped ts;
ts.header.frame_id="world";
ts.header.stamp=ros::Time::now();
ts.child_frame_id="turtle1";
ts.transform.translation.x=pose->x;
ts.transform.translation.y=pose->y;
ts.transform.translation.z=0;
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose->theta);
ts.transform.rotation.x=qtn.getX();
ts.transform.rotation.y=qtn.getY();
ts.transform.rotation.z=qtn.getZ();
ts.transform.rotation.w=qtn.getW();
broadcaster.sendTransform(ts);
ROS_INFO("JINLAILE");
}
int main(int argc, char* argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"dynamic_tf_pub");
ros::NodeHandle nh;
ros::Subscriber sub=nh.subscribe("/turtle1/pose",1000,doPose);
ros::spin();
return 0;
} 当运行turtlesim节点和键盘控制节点时，控制小乌龟在界面上运动的时候，
通过rviz可以看到小乌龟身上的坐标系在跑：
参考链接：
http://www.autolabor.com.cn/book/ROSTutorials/di-5-zhang-ji-qi-ren-dao-hang/51-tfzuo-biao-bian-huan/511-zuo-biao-msg-xiao-xi.html