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 <vector>
#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 <vector>
#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 <ros/ros.h>
#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<turtlesim::Pose>("/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