TF

tf是一个让用户随时间跟踪多个参考系的功能包，它使用一种树型数据结构，根据时间缓冲并维护多个参考系之间的坐标变换关系，可以帮助用户在任意时间，将点、向量等数据的坐标，在两个参考系中完成坐标变换。

简单来说，我们知道在机器人工作过程中，机器人自身处于世界坐标系下，同时其自身又具备了一个局部坐标系，这两个坐标系的存在确保了我们能够计算得出机器人的位姿。

基本数据类型

分别对应四元数，向量，点坐标，位姿和转换模板

tf::Stamped <T>

   1 template <typename T>2 class Stamped : public T{3  public:4   ros::Time stamp_;5   std::string frame_id_;6 7   Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation8 9   Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id);10   11   void setData(const T& input);12 };

在上述所有基本类型（除了tf::Transform）的基础上具有元素frame_id_和stamp_模板化

tf::StampedTransform(这个类型在激光slam中应用很多)

该类型是tf::Transform的一个特例，同时要求具有frame_id，stamp 和 child_frame_id

   1 /** \brief The Stamped Transform datatype used by tf */2 class StampedTransform : public tf::Transform3 {4 public:5   ros::Time stamp_; ///< The timestamp associated with this transform                                                                                                                                                                                                                                                        6   std::string frame_id_; ///< The frame_id of the coordinate frame  in which this transform is defined                                                                                                                                                                                                                       7   std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines                                                                                                                                                                                                                              8   StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):9     tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ };10 11   /** \brief Default constructor only to be used for preallocation */12   StampedTransform() { };13 14   /** \brief Set the inherited Traonsform data */15   void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;};16 17 };

监听和广播

广播

#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<turtlesim/Pose.h>std::string turtle_name;void poseCallback(const turtlesim::PoseConstPtr& msg)
{ROS_INFO("pose:%0.2f,%0.2f,%0.2f",msg->x,msg->y,msg->theta);//tf广播定义static tf::TransformBroadcaster br;//广播两个坐标系的变换存储在transform里tf::Transform transform;//初始化t和q平移和旋转transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));tf::Quaternion q;q.setRPY(0,0,msg->theta);transform.setRotation(q);//广播 将world坐标系和turtle_name坐标系的变换存储在transform中，ros::Time::now()为时间戳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];ros::NodeHandle node;ros::Subscriber sub = node.subscribe(turtle_name+"/pose",10,poseCallback);ros::spin();return 0;
}

监听

#include<ros/ros.h>
#include<tf/transform_listener.h>
#include<geometry_msgs/Twist.h>
#include<turtlesim/Spawn.h>int main(int argc,char**argv)
{ros::init(argc,argv,"my_tf_listener");ros::NodeHandle node;ros::service::waitForService("/spawn");ros::ServiceClient add_turtle=node.serviceClient<turtlesim::Spawn>("/spawn");turtlesim::Spawn srv;add_turtle.call(srv);ros::Publisher turtle_vel=node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",10);tf::TransformListener listener;ros::Rate rate(10);while(node.ok()){tf::StampedTransform transform;try{//等待变换，也就是等待二者的上线，并且可以变换（在一棵TF树上），超过三秒就报错listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));//将turtle1和turtle2的坐标系变换储存在transform中listener.lookupTransform("/turtle2","turtle1",ros::Time(0),transform);}catch(tf::TransformException &ex){ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}geometry_msgs::Twist vel_msg;//通过计算turtle1相对于turtle2的位置的正切值，来计算turtle2需要旋转的角度，使得turtle1位于turtle2单位正前方。4作为一个系数vel_msg.angular.z=4.0*atan2(transform.getOrigin().y(),transform.getOrigin().x());//x方向（即正前方）的速度与两只海龟之间的距离有关，距离越大速度越大。vel_msg.linear.x=0.5*sqrt(pow(transform.getOrigin().x(),2)+pow(transform.getOrigin().y(),2));turtle_vel.publish(vel_msg);rate.sleep();}return 0;
}

LIO-SAM中的TF应用

监听

首先定义监听器和雷达系到载体系的坐标变换

    tf::TransformListener tfListener;tf::StampedTransform lidar2Baselink;

之后便同上面代码一样等待变换，接收雷达到载体的坐标系变换

// 等待3s
tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
// lidar系到baselink系的变换
tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);

广播

定义广播

static tf::TransformBroadcaster tfOdom2BaseLink;

将载体系和里程计系的坐标变换发送到tCur

tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
tfOdom2BaseLink.sendTransform(odom_2_baselink);

参数使用

LIO-SAM的参数定义在

其中部分参数如下：

yaml文件数据格式

前面对应的是参数的名称后面是参数的值

# TopicspointCloudTopic: "/velodyne_points"               # Point cloud dataimuTopic: "/airsim_node/drone_1/imu/Imu"                         # IMU data# imuTopic: "imu_correct"odomTopic: "odometry/imu"                   # IMU pre-preintegration odometry, same frequency as IMUgpsTopic: "odometry/gpsz"                   # GPS odometry topic from navsat, see module_navsat.launch file

yaml文件加载

要使用这些参数必须通过launch文件加载

<!-- Parameters --><rosparam file="$(find lio_sam)/config/params.yaml" command="load" />

同时在utility.h中也为这些参数提供了一些默认值

nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");
nh.param<std::string>("lio_sam/imuTopic", imuTopic, "imu_correct");
nh.param<std::string>("lio_sam/odomTopic", odomTopic, "odometry/imu");
nh.param<std::string>("lio_sam/gpsTopic", gpsTopic, "odometry/gps");

其中第二个参数是参数的名称；第一个参数是从launch文件加载的参数文件(yaml)中有 pointCloudTopic定义的值则用launch中加载来的否则用默认值points_raw

yaml参数调用

subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());

第一个参数为话题名；已经在yaml文件中给他定义了值为/velodyne_points，第二个参数是队列长度，第三个参数为回调函数(一般为加载和处理雷达数据)

话题

ros话题通信模型如上；话题订阅和发布的模型如下

1、创建发布者

2、创建发布者

3、添加编译选项

4、运行可执行程序

发布者实现

发布者实现流程

初始化ros节点

向rosmaser注册节点信息，包括发布话题名和话题消息类型

按一定频率发布消息类型

#include <sstream>
#include "ros/ros.h"
#include "std_msgs/String.h"int main(int argc, char **argv)
{// ROS节点初始化ros::init(argc, argv, "talker");// 创建节点句柄ros::NodeHandle n;// 创建一个Publisher，发布名为chatter的topic，消息类型为std_msgs::String//1000为队列长度，用于存储ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);// 设置循环的频率//发送消息的频率为10Hz， 1秒发10个，周期为0.1sros::Rate loop_rate(10);int count = 0;while (ros::ok()){// 初始化std_msgs::String类型的消息std_msgs::String msg;std::stringstream ss;ss << "hello world " << count;msg.data = ss.str();// 发布消息ROS_INFO("%s", msg.data.c_str());chatter_pub.publish(msg);// 循环等待回调函数ros::spinOnce();// 按照循环频率延时loop_rate.sleep();++count;}return 0;
}

订阅者实现流程

初始化ros节点

订阅需要的话题

循环等待话题消息，接收到消息后进入回调函数

在回调函数中完成消息处理

#include "ros/ros.h"
#include "std_msgs/String.h"// 接收到订阅的消息后，会进入消息回调函数
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{// 将接收到的消息打印出来ROS_INFO("I heard: [%s]", msg->data.c_str());
}int main(int argc, char **argv)
{// 初始化ROS节点ros::init(argc, argv, "listener");// 创建节点句柄ros::NodeHandle n;// 创建一个Subscriber，订阅名为chatter的topic，注册回调函数chatterCallbackros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);// 循环等待回调函数ros::spin();return 0;
}

自定义消息话题

当ros的一些自定义话题不能满足我们的需要时我们可以自定义一些话题类型

首先要在功能包里创建一个msg文件夹；创建一个msg文件比如：

string name
uint8 sex
uint8 ageuint8 unknown = 0
uint8 male    = 1
uint8 female  = 2

在功能包下的package.xml文家里添加

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

在CMakeLists中添加

find_package(catkin REQUIRED COMPONENTSgeometry_msgsroscpprospystd_msgsturtlesimmessage_generation # 这是新加的
)add_message_files(FILES Person.msg)
generate_messages(DEPENDENCIES std_msgs)catkin_package(# INCLUDE_DIRS include# LIBRARIES learning_topicCATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime# DEPENDS system_lib
)

之后catkin_make就会在devel/include下生成一个.h文件

发布者

和发布ros自定义的消息类似；只是消发布息类型变为learning_topic::Person

learning_topic是功能包的名称

消息初始化时如下：

learning_topic::Person person_msg;
person_msg.name = "Tom";
person_msg.age = 18;
person_msg.sex = learning_topic::Person::male;

/* 该例程将发布“/person_info”话题，自定义消息类型“learning_topic::Person” */
#include <ros/ros.h>
#include "learning_topic/Person.h"int main ( int argc, char **argv ) {ros::init ( argc, argv, "person_publisher" ); /* ROS节点初始化 */ros::NodeHandle n; /* 创建节点句柄 *//* 创建一个Publisher，发布名为“/person_info”的topic，消息类型为“learning_topic::Person”，队列长度10 */ros::Publisher person_info_pub = n.advertise<learning_topic::Person> ( "/person_info", 10 );ros::Rate loop_rate ( 1 ); /* 设置循环的频率 */while ( ros::ok() ) {/* 初始化learning_topic::Person类型的消息 */learning_topic::Person person_msg;person_msg.name = "Tom";person_msg.age  = 18;person_msg.sex  = learning_topic::Person::male;person_info_pub.publish ( person_msg ); /* 发布消息 */ROS_INFO ( "Publish Person Info: name:%s  age:%d  sex:%d",person_msg.name.c_str(), person_msg.age, person_msg.sex );loop_rate.sleep(); /* 按照循环频率延时 */}return 0;
}

/* 该例程将订阅“/person_info”话题，自定义消息类型“learning_topic::Person” */
#include <ros/ros.h>
#include "learning_topic/Person.h"/* 接收到订阅的消息后，会进入消息回调函数 */
void personInfoCallback ( const learning_topic::Person::ConstPtr& msg ) {/* 将接收到的消息打印出来 */ROS_INFO ( "Subcribe Person Info: name:%s  age:%d  sex:%d",msg->name.c_str(), msg->age, msg->sex );
}int main ( int argc, char **argv ) {ros::init ( argc, argv, "person_subscriber" ); /* 初始化ROS节点 */ros::NodeHandle n; /* 创建节点句柄 *//* 创建一个Subscriber，订阅名为“/person_info”的topic，注册回调函数personInfoCallback */ros::Subscriber person_info_sub = n.subscribe ( "/person_info", 10, personInfoCallback );ros::spin(); /* 循环等待回调函数 */return 0;
}

LIO-SAM中msg的使用

消息类型定义

LIO-SAM中也定义了用于发布的ros话题类型

# Cloud Info
Header header int32[] startRingIndex
int32[] endRingIndexint32[]  pointColInd # point column index in range image
float32[] pointRange # point range int64 imuAvailable
int64 odomAvailable# Attitude for LOAM initialization
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit# Initial guess from imu pre-integration
float32 initialGuessX
float32 initialGuessY
float32 initialGuessZ
float32 initialGuessRoll
float32 initialGuessPitch
float32 initialGuessYaw# Point cloud messages
sensor_msgs/PointCloud2 cloud_deskewed  # original cloud deskewed
sensor_msgs/PointCloud2 cloud_corner    # extracted corner feature
sensor_msgs/PointCloud2 cloud_surface   # extracted surface feature

发布和订阅

由于代码太长此处只展示一个订阅一个发布

// 订阅当前激光帧运动畸变校正后的点云信息
subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
// 发布当前激光帧提取特征之后的点云信息
pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 1);

可以看到和发布的消息类型都是lio_sam::cloud_info

在进行话题内部数据定义时如下：

//定义
lio_sam::cloud_info cloudInfo;
//做一些赋值初始化等操作
cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;
cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;
cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;

std_msgs

简介

std_msgs中包含标准的ros消息类型；

节点之间通过发布话题和订阅话题来通信，在程序中是通过消息发布器和订阅器来实现，数据流通过话题的发布和订阅在节点之间传播，而数据流的数据类型则称为消息

roswiki的官方文档如下：

std_msgs - ROS Wiki

消息类型种类

Header

例如在激光slam中传递接收较多的一个消息类型Header：点开查看可以看到

该消息类型主要用于在一个特定的坐标系下交流时间戳

LIO-SAM中的应用

std_msgs::Header cloudHeader
// get timestamp
// 当前帧头部
cloudHeader = currentCloudMsg.header;
//当前帧起始时刻
timeScanCur = cloudHeader.stamp.toSec();

ros学习 tf；参数；话题；std_msgs相关推荐

（18）ROS学习-TF坐标变换之静态坐标变换
1.坐标msg消息: 订阅发布模型中数据载体 msg 是一个重要实现,首先需要了解一下,在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped和geometry_ ...
ROS学习-tf介绍
tf简介一个机器人系统一个时间段通常有多个3D坐标系在变化,如世界全局坐标系,world frame, base frame, gripper frame(手臂/夹子坐标框架),head frame ...
周末ROS学习沙龙第一期——ROS历史、安装、消息话题节点服务等概念、SLAM导航框架及参数、小车上运行SLAM
非博主原创,出于方便学习的目的,将周末ROS学习沙龙www.corvin.cn的课堂讲义PPT整理在这(老师讲得超棒!),无任何盈利目的,若有侵权则删除. Ros小课堂链接:https://space ...
ROS学习小笔记(Topic通信，service通信，参数服务器)
(菜鸡整理的学习笔记,原文链接已经挂上,不小心过来的朋友建议去博主那边学习,谢谢) 原文链接:https://blog.csdn.net/LoongEmbedded/article/details/1 ...
ROS学习（一）——话题通信的C++与Python实现
ROS学习(一)--话题通信的C++与Python实现引言:本文分为三个部分,分别为话题通信的理论模型以及其C++与Python的实现代码. 一.理论模型在我们的实际使用中,需要注意的几个东西就是 ...
ROS通信机制：话题、服务、参数
目录话题通信理论模型流程通信样例自定义消息的通信服务通信理论模型服务通信自定义srv 参数服务器理论模型参数操作话题通信话题通信是ROS中使用频率最高的一种通信模式,话题通信是 ...
ROS学习【2】-----ubuntu16.04中进行ROS通信编程(话题编程)
ROS通信编程-----话题编程作为嵌入式系统应用开发的重要支柱之一的ROS,是我们学习嵌入式开发的第一步,既要学习ROS,我们就得知道ROS怎么使用,今天,林君学长带大家开始学习ROS的第一步:R ...
ROS学习笔记六：理解ROS服务和参数
ROS学习笔记六:理解ROS服务和参数主要介绍ROS服务和参数,同时使用命令行工具rosservice和rosparam. ROS service service是节点之间互相通信的另一种方式,se ...
ROS学习之tf在rviz中的显示
昨天粗略看了一下ros中rviz的用法.事实,他就是一个可视化的工具.wiki的用户手册:http://wiki.ros.org/rviz/UserGuide 今天在学习tf的教程时,首先是演示了一个 ...

ros学习 tf；参数；话题；std_msgs

TF