ROS:TF笔记

TF坐标变换，实现不同类型的坐标系之间的转换

tf2常用功能包有:

tf2_geometry_msgs:可以将ROS消息转换成tf2消息。

tf2: 封装了坐标变换的常用消息。

tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。

TF中常用的msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped

geometry_msgs/TransformStamped

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/Transform transform
  geometry_msgs/Vector3 translation
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion rotation
    float64 x
    float64 y
    float64 z
    float64 w

geometry_msgs/PointStamped

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Point point
  float64 x
  float64 y
  float64 z

静态坐标变换

发布坐标转换

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf/LinearMath/Quaternion.h"
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "static_pub");
    ros::NodeHandle n;
    tf2_ros::StaticTransformBroadcaster pub; //创建发布对象
    geometry_msgs::TransformStamped tfs;  //创建坐标转换消息
    tfs.header.stamp = ros::Time::now();
    tfs.header.frame_id = "base_link";
    tfs.child_frame_id = "laser";
    tfs.transform.translation.x = 0.2;
    tfs.transform.translation.y = 0;
    tfs.transform.translation.z = 0.5;
    tf::Quaternion qtn;  //通过欧拉角设置四元数 包含"tf/LinearMath/Quaternion.h"
    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;
}

#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, "static_sub");
    ros::NodeHandle n;
    tf2_ros::Buffer buffer;  //创建buffer对象,存储坐标转换
    tf2_ros::TransformListener listener(buffer);  //创建接收对象,传入buffer
    geometry_msgs::PointStamped p;  //创建点
    p.header.frame_id = "laser";
    p.header.stamp = ros::Time::now();
    p.point.x = 2.0;
    p.point.y = 3.0;
    p.point.z = 5.0;
    // ros::Duration(2).sleep();  //方案1:休眠解决"base_link" passed to lookupTransform argument target_frame does not exist.  

    ros::Rate rate(10);
    while (ros::ok())
    {
        geometry_msgs::PointStamped p_out;
        try  //方案2:try解决
        {
            p_out = buffer.transform(p, "base_link"); //需包含"tf2_geometry_msgs/tf2_geometry_msgs.h"
            ROS_INFO("\033[1;32m%.2f %.2f %.2f 参考坐标系:%s\033[0m", p_out.point.x,
            p_out.point.y, p_out.point.z, p_out.header.frame_id.c_str());
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("\033[31m %s \033[0m", e.what());
        }
        

        rate.sleep();
        ros::spinOnce();
    }
    

    return 0;
}

直接调用内置功能包实现

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

动态坐标变换

订阅小乌龟位姿发布to world的坐标变换在rviz中显示

#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& msg){
    static tf2_ros::TransformBroadcaster pub;
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "turtle1";
    ts.transform.translation.x = msg->x;
    ts.transform.translation.y = msg->y;
    ts.transform.translation.z = 0;
    tf2::Quaternion qtn;
    qtn.setRPY(0, 0, msg->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();
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "dynamic_pub");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/turtle1/pose", 100, doPose);
    ros::spin();
    return 0;
}

多坐标变换

现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标

launch文件调用static_transform_publisher发布静态坐标变换

<launch>
    <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son1" args = "5 0 0 0 0 0 /world /son1" output = "screen" />
    <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son2" args = "3 0 0 0 0 0 /world /son2" output = "screen" />

</launch>

rviz

 订阅方实现

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/TransformStamped.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "multi_tf_sub");
    ros::NodeHandle n;
    tf2_ros::Buffer buf;
    tf2_ros::TransformListener sub(buf);
    geometry_msgs::PointStamped pAtSon1; //创建son1下坐标点
    pAtSon1.header.stamp = ros::Time::now();
    pAtSon1.header.frame_id = "son1";
    pAtSon1.point.x = 1;
    pAtSon1.point.y = 1;
    pAtSon1.point.z = 1;

    ros::Rate rate(10);
    while(ros::ok()){
        try
        {
            geometry_msgs::TransformStamped son1Toson2 = buf.lookupTransform("son2", "son1", ros::Time(0)); //son2:目标坐标系 son1:原坐标系 ros::Time(0):取间隔最短的两个坐标关系帧计算相对关系
            ROS_INFO("原坐标系:%s, 现坐标系:%s, 偏移:%.2f, %.2f, %.2f",
                     son1Toson2.child_frame_id.c_str(),
                     son1Toson2.header.frame_id.c_str(),
                     son1Toson2.transform.translation.x,
                     son1Toson2.transform.translation.y,
                     son1Toson2.transform.translation.z);
            geometry_msgs::PointStamped pAtSon2 = buf.transform(pAtSon1, "son2");
            ROS_INFO("所属坐标系:%s, x:%.2f, y:%.2f, z:%.2f", 
                     pAtSon2.header.frame_id.c_str(),
                     pAtSon2.point.x,
                     pAtSon2.point.y,
                     pAtSon2.point.z);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("\033[31m 错误提示:%s \033[0m", e.what());
        }
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

[ INFO] [1652065692.312667393]:  错误提示:"son2" passed to lookupTransform argument target_frame does not exist.  
[ INFO] [1652065692.412696461]:  错误提示:"son2" passed to lookupTransform argument target_frame does not exist.  
[ INFO] [1652065692.512686668]:  错误提示:"son2" passed to lookupTransform argument target_frame does not exist.  
[ INFO] [1652065692.612818055]: 原坐标系:son1, 现坐标系:son2, 偏移:2.00, 0.00, 0.00
[ INFO] [1652065692.612945493]: 所属坐标系:son2, x:3.00, y:1.00, z:1.00
[ INFO] [1652065692.712710755]: 原坐标系:son1, 现坐标系:son2, 偏移:2.00, 0.00, 0.00
[ INFO] [1652065692.712834071]: 所属坐标系:son2, x:3.00, y:1.00, z:1.00
[ INFO] [1652065692.812684046]: 原坐标系:son1, 现坐标系:son2, 偏移:2.00, 0.00, 0.00