ROS TF坐标变换实践，C++实现

ROS TF坐标变换实践

ROS中将坐标变换封装，用到的常用的功能包有以下3种。
tf2_geometry_msgs:可以将ROS消息转换成tf2消息。
tf2:封装了坐标变换的常用消息。
tf2_ros:封装了坐标变换的API。

坐标msg消息

geometry_msgs/TransformStamped
geometry_msgs/PointsStamped

通过rosmsg命令可以查看消息组成，以下命令可以查看。

rosmsg info geometry_msgs/TransformStamped
rosmsg info geometry_msgs/PointsStamped

静态坐标变换 C++实现 发布方

静态坐标是指两个坐标系之间的相对位置是固定的。

#include "ros/ros.h"
#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,"static_pub");
    ros::NodeHandle nh;
    tf2_ros::StaticTransformBroadcaster pub;
    geometry_msgs::TransformStamped tfs;
    tfs.header.stamp = ros::Time::now();
    tfs.header.frame_id = "baselink";//坐标系中被参考的主体
    tfs.child_frame_id = "laser";

    //两个坐标系的相对位置偏移量设置
    tfs.transform.translation.x=0.2;
    tfs.transform.translation.y=0.0;
    tfs.transform.translation.z=0.5;
    //需要欧拉角转换
    tf2::Quaternion qtn;//创建四元数
    //向该对象设置欧拉角，这个对象可以将欧拉角转换成四元数
    qtn.setRPY(0,0,0);//单位弧度
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();//将欧拉角转换成四元数
    pub.sendTransform(tfs);//发布数据
    ros::spin();
    return 0;
}

静态坐标变换 C++实现 订阅方

#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "ros/ros.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
int main(int argc,char *argv[])
{
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    //create a buffer to store the msgs.
    tf2_ros::Buffer buffer;
    //create a listener buffer.
    tf2_ros::TransformListener listener(buffer);
    //create data of point.
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "laser";
    ps.header.stamp = ros::Time::now();
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    //转换算算法
    ros::Rate rate(10);
    //添加休眠
    ros::Duration(2).sleep();
    while(ros::ok())
    {
        //将ps转换成相对于baselink的坐标
        auto ps_out = buffer.transform(ps,"baselink"); //转换坐标
        ROS_INFO("the transformed value %.2f,%.2f,%.2f,%s",ps_out.point.x, ps_out.point.y,ps_out.point.z,ps_out.header.frame_id.c_str());//打印输出
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

写代码时遇到到的坑，编译时报错 undefined tf2

遇到报undefined tf2，检查是否正确配置编译文件。

catkin_create_pkg pkg_name pkg_dependencies

创建新的包时，忘记创建tf2_ros，tf2_geometry_msgs, geometry_msgs。后续需要自己配置。
不然在编译代码时会报相应的错误。

CmakeLists.txt中添加如下配置：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  tf2_ros
  tf2_geometry_msgs
  geometry_msgs
)

在package.xml中添加如下配置

  <build_depend>tf2</build_depend>
  <build_depend>tf2_ros</build_depend>
  <build_depend>tf2_geometry_msgs</build_depend>
 
  <exec_depend>tf2</exec_depend>
  <exec_depend>tf2_ros</exec_depend>
  <exec_depend>tf2_geometry_msgs</exec_depend>