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

TF

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

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

基本数据类型

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

tf::Stamped

   1 template 
   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 preallocation
   8 
   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::Transform
   3 {
   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(this) = input;};
  16 
  17 };

监听和广播

广播

#include
#include
#include

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
#include
#include
#include

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("/spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    ros::Publisher turtle_vel=node.advertise("/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文件数据格式

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

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

yaml文件加载

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

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

nh.param("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");
nh.param("lio_sam/imuTopic", imuTopic, "imu_correct");
nh.param("lio_sam/odomTopic", odomTopic, "odometry/imu");
nh.param("lio_sam/gpsTopic", gpsTopic, "odometry/gps");

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

 yaml参数调用

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

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

话题

​

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

1、创建发布者

2、创建发布者

3、添加编译选项

4、运行可执行程序

发布者实现

发布者实现流程

初始化ros节点

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

按一定频率发布消息类型

#include 
#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("chatter", 1000);

  // 设置循环的频率//发送消息的频率为10Hz， 1秒发10个，周期为0.1s
  ros::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，注册回调函数chatterCallback
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

  // 循环等待回调函数
  ros::spin();

  return 0;
}

自定义消息话题

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

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

string name
uint8 sex
uint8 age

uint8 unknown = 0
uint8 male    = 1
uint8 female  = 2

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

message_generation
message_runtime

在CMakeLists中添加

find_package(catkin REQUIRED COMPONENTS
    geometry_msgs
    roscpp
    rospy
    std_msgs
    turtlesim
    message_generation # 这是新加的
)

add_message_files(FILES Person.msg)
generate_messages(DEPENDENCIES std_msgs)

catkin_package(
    # INCLUDE_DIRS include
    # LIBRARIES learning_topic
    CATKIN_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 
#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 ( "/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;
}

订阅

回调函数稍有不同

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 );
}

订阅消息类型learning_topic::Person::ConstPtr& msg

/* 该例程将订阅“/person_info”话题，自定义消息类型“learning_topic::Person” */
#include 
#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[] endRingIndex

int32[]  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/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
// 发布当前激光帧提取特征之后的点云信息
pubLaserCloudInfo = nh.advertise ("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();