ROS当中TF坐标系是怎么发布和管理的

一.ROS坐标系的发布

千言万语离不开一句话tfBroadcaster.sendTransform(odomTrans);

        1.其中tfBroadcaster为专门用来发布广播的对象.

                需要进行这样的声明tf::TransformBroadcaster tfBroadcaster;

        2.odomTrans则包含了坐标之间的关系信息.

                他是需要这样声明的nav_msgs::Odometry odomData;                

        3.坐标系描述谁的关系,关系咋样?这个要说清楚

odomTrans.frame_id_ = "map";          //全局坐标
odomTrans.child_frame_id_ = "sensor"; //本地坐标的id,也就是自己
odomTrans.stamp_ = odomTime;          //时间戳
odomTrans.setRotation(tf::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w));
                                      //发布四元数(旋转)
odomTrans.setOrigin(tf::Vector3(vehicleX, vehicleY, vehicleZ));
                                      //发布xyz偏移量(平移)

        4.发布

PS:旋转平移的相关参数是没有办法直接获得的,它们是通过imu,激光雷达估计或者其他方式进行推断的.

二.世界坐标系?

        值得一提的是其实对于ros来说没有所谓的世界坐标系.

例如:

        1.你可以以"世界坐标系"为世界坐标系,也可以以机器人的base_link为世界坐标

                    

        2.但是可以以fram_id来划分不同的世界,这个和上面在同一个rviz可以调出来.根据母frame(parent_frame)划分不同的世界.一个母frame一个世界.

 

 三.机器人身上坐标系太多,好难划分

        机器人通过第一个标题将base_link与世界坐标系绑定后,后续再有像是激光雷达和camera这种定死在base_link上面的可以通过tf包来绑定,这样就不用重复地发布tf坐标了.

  <node pkg="tf" type="static_transform_publisher" name="vehicleTransPublisher" 
args="-$(arg sensorOffsetX) -$(arg sensorOffsetY) 0 0 0 0 /sensor /vehicle 1000"/>

name表示tf这个可执行文件以什么名字作为节点的名称向tf工具来发布话题

args前6个分别是相对于base_link的平移和旋转量

/sensor是base_link

/vehicle是传感器的名称,这里可以理解为camera

甚至可以莫须有一个坐标系出来,它不依托物质基础,你想在哪里设置坐标系就设置一个

像下面这张图黄色这根线长出天际,是因为我在其很高的上方设置了一个坐标系.

四.怎么利用tf坐标系

以古月居的广播教学为例子https://www.guyuehome.com/34664

turtle_tf_listener.cpp中

listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));                    
                                                                //等待响应
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
                                                                //坐标信息交给transform
transform.getOrigin().y()    //获得目标y坐标
transform.getOrigin().x()    //获得目标x坐标
transform.getOrigin().z()    //获得目标z坐标
w = transform.getRotation().getW();  //获得四元数
x = transform.getRotation().getX();
y = transform.getRotation().getY();
z = transform.getRotation().getZ();

/*四元数转rpy*/
tf::Quaternion q(x,y,z,w);
double roll, pitch, yaw;//定义存储r\p\y的容器
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);//进行转换

参考链接:1.tf(Transform Frame)变换_wanghua609的博客-CSDN博客_lookuptransform

2.PCL:旋转、平移点云_通哈膨胀哈哈哈的博客-CSDN博客_pcl点云旋转

 下面这个是将2D雷达进行旋转平移的例子,代码在下方

其中有意思的是:

1.由于消息是从gazebo发出来的,似乎直接接受到的话题消息不需要进行旋转(而现实当中是需要旋转的)

图(一)

图(二)

         左边白色为map坐标系下的点云信息;右边为雷达坐标系下的点云信息(未经过旋转平移,所以点云分布在原点附近).

        两图的小车方向显然发生了变化(可以看Axes红绿蓝三轴的朝向不同了),但是点云并不会以小车进行旋转.=>因此这里点云只需要进行平移,不需要进行旋转.

2.代码分析

        listener.lookupTransform("/map", "/sensor", now, transform);

        这句话是为了获取 transform的,我们发现点云都分布在map原点附近,而不是分布在小车附近,这是我们需要改变的点.因此我们需要把所有点云坐标加上从原点指到小车的这个向量,那么就可以完成我们的需求.因此这句代码targe_frame是"/map",source_frame为"/sensor".这样能完成点云从sensor坐标系向map坐标系的变换.

x=transform.getOrigin().x();
y=transform.getOrigin().y();
z=transform.getOrigin().z();
transform_me.translation() << x, y, 0;
pcl::transformPointCloud (cloud, cloud, transform_me);

 3.重要的东西!!!时间戳

        如果说将接受到的雷达信息传进来,再进行坐标变换,那么从雷达信息生成的时间点到开始进行坐标变换的这个时刻存在时间差!!!!会造成定位不准,白瞎了激光雷达的性能.

        因此,要保证雷达点云生成的时间戳和tf关系的时间戳尽量接近.

ros::Time now = input.header.stamp;
listener.lookupTransform("/map", "/sensor", now, transform);

         将等待tf时间戳的输入设为雷达信息的时间戳stamp.


//这段代码订阅laserscan转成pointcloud2
#include <ros/ros.h>
#include <iostream>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl/filters/voxel_grid.h>
#include <tf/transform_listener.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/passthrough.h>
// #include <geometry_msgs/TwistStamped.h>
#define PI 3.14159265359
std::vector<int> scanInd;
class cloudHandler
{
public:
    pcl::PointCloud<pcl::PointXYZI> cloud;
    pcl::PointCloud<pcl::PointXYZI> copy_cloud;
    pcl::PointCloud<pcl::PointXYZI> TFED_cloud;
    tf::TransformListener listener;
    tf::StampedTransform transform;
    double x,y,z;
    double roll, pitch, yaw;
    cloudHandler()
    {
        pcl_sub = nh.subscribe("/2D_Lascan", 100, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/registered_scan_tem", 2);
    }
    void cloudCB(const sensor_msgs::LaserScan &input)                                 //将ros消息转为pcl点云格式后将其在viwer窗口上表现出来
    {
        cloud.clear();
        copy_cloud.clear();
        std::vector<float> ranges = input.ranges;
        cloud.points.resize(ranges.size());                                                     
                                            //??   这里给pcl2声明容量 
        cloud.width = ranges.size();
                                            //??   这里给pcl2 width,heighy赋值
        cloud.height = 1;
        cloud.header.stamp = input.header.stamp.toSec();
                                            //??   这里给pcl2时间戳赋值
        cloud.header.frame_id = "map";
                                            //??   这里给pcl2设置发送ID
        /*--------------这段进行"角度range"到(x,y)坐标的转换---------------*/
        //转换到二维XY平面坐标系下;
        // std::cout<<"cloud其他初始化"<<std::endl;
        for(int i=0; i< ranges.size(); i++)
        {
            double angle = input.angle_min + i * input.angle_increment;
            double lX = ranges[i] * cos(angle);
            double lY = ranges[i] * sin(angle);
            float intensity = input.intensities[i];
            // std::cout << ranges[i] << " , " << std::endl;
            cloud.points[i].x=lX;
            cloud.points[i].y=lY;
            cloud.points[i].z=0;
            cloud.points[i].intensity=intensity;
        }
        /*--------------这段进行"角度range"到(x,y)坐标的转换---------------*/
        /*----------------------------------------------------旋转平移----------------------------------------------------------------*/
        try
        {
            //查询是否有这两个坐标系,查询当前时间,如果超过3s则报错
            ros::Time now = input.header.stamp;
            std::cout<<now<<std::endl;
            std::cout<<ros::Time(0)<<std::endl;
            listener.waitForTransform("/map", "/sensor", now, ros::Duration(3.0));
            listener.lookupTransform("/map", "/sensor", now, transform);                //将点云加上map到激光雷达的向量,进行平移
            x=transform.getOrigin().x();
            y=transform.getOrigin().y();
            z=transform.getOrigin().z();
            tf::Quaternion q(transform.getRotation().getX(),transform.getRotation().getY(),transform.getRotation().getZ(),transform.getRotation().getW());
            tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
            Eigen::Affine3f transform_me = Eigen::Affine3f::Identity();
            transform_me.translation() << x, y, 0;                                      //将点云加上map到激光雷达的向量,进行平移
            // transform_me.rotate (Eigen::AngleAxisf (roll, Eigen::Vector3f::UnitX()));//这边不需要旋转,因为2D雷达的消息直接和map的方向对上了,这我也很迷为什么对上了
            pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
            pcl::transformPointCloud (cloud, cloud, transform_me);
            /*------------------------------增加立体感------------------------------------------*/
            pcl::copyPointCloud(cloud,copy_cloud);
            for(int h=0;h<=20;h+=2)
            {
                for(int i=0;i<copy_cloud.points.size();i++)
                {
                    copy_cloud.points[i].z=(float)h/10;
                }
                cloud+=copy_cloud;
            }
            /*------------------------------增加立体感------------------------------------------*/
            sensor_msgs::PointCloud2 cloud_pu;
            pcl::toROSMsg(cloud, cloud_pu);                       
            pcl_pub.publish(cloud_pu);

        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s",ex.what());
            // ros::Duration(1.0).sleep();
            // continue;
        }
        /*----------------------------------------------------旋转平移----------------------------------------------------------------*/
    }
protected:
    ros::NodeHandle nh;
    ros::Subscriber pcl_sub;
    ros::Subscriber subSpeed;
    ros::Subscriber subTerrainCloud;
    pcl::visualization::CloudViewer viewer;                                             //标题
    ros::Timer viewer_timer;
    ros::Publisher pcl_pub;
};
main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_visualize");
    cloudHandler handler;
    ros::spin();
    return 0;
}

五.怎么获取base_link相对于世界坐标系的偏移量?

<think>好的,我现在需要帮助用户了解ROS坐标系之间的连接方式转换方法。根据用户提供的引用资料,我得先整理一下相关信息,确保回答准确且符合用户的需求。 首先,用户提到了几个引用,我需要逐一分析这些引用中的关键点。引用[1]提到了ROS中的坐标系知识,特别是REP 105标准,这可能涉及到标准坐标系的定义。引用[2]详细解释了TF的工作原理,包括如何通过订阅/tf话题来获取坐标系之间的变换关系,以及如何通过路径搜索找到两个坐标系之间的转换路径。引用[3]讨论了数据类型转换函数,比如四元数、旋转矩阵等,还提到了TF类的使用,包括发布订阅器的接口。引用[4]提到了数据类型转换,比如RPY到四元数的转换。 接下来,我需要将这些信息组织成一个结构清晰的回答,涵盖用户关心的连接方式转换方法。可能的步骤包括: 1. **坐标系连接方式**:解释TF树的结构,父子坐标系的关系,以及如何通过发布变换来建立连接。这里需要提到TransformBroadcaster发送变换信息到/tf话题。 2. **转换方法**:说明如何获取两个坐标系之间的变换关系,使用TransformListener订阅/tf,然后调用lookupTransform函数。同时,可能需要处理异常,比如坐标系不存在或时间不同步的情况。 3. **数据类型与转换函数**:介绍ROS中使用的数据类型(如四元数、旋转矩阵、欧拉角等),以及如何在它们之间进行转换,比如使用tf库中的转换函数。 4. **代码示例**:给出具体的Python或C++代码示例,展示如何发布监听坐标变换,以及如何进行数据类型的转换。这有助于用户实际操作。 5. **注意事项应用场景**:提醒用户关于时间戳同步、TF树的维护,以及常见应用场景,如传感器数据融合、导航机械臂控制等。 在整理过程中,需要注意引用对应的参考文献,比如在解释TF树时引用[2],在数据类型转换时引用[3][4]。同时,确保使用正确的中文术语,避免技术术语错误。 可能遇到的问题包括如何确保时间戳的一致性,如何处理多个坐标系的复杂变换链,以及数据类型转换时的常见错误。这些都需要在回答中适当提及,并提供解决方案或参考链接。 最后,生成的相关问题需要与ROS坐标系相关,比如优化转换效率的方法、与其他工具的比较、调试技巧等,这样用户如果有进一步的问题,可以自行查阅或继续提问。</think>### ROS坐标系连接与转换方法详解 #### 一、坐标系连接方式 1. **父子坐标系关系** ROS通过**TF树**管理坐标系间的层级关系,每个坐标系必须有一个父坐标系(除根坐标系外)。例如,机器人底盘`base_link`的父坐标系可以是地图`map`,激光雷达`lidar`的父坐标系可以是底盘`base_link`。这种层级关系通过发布**静态或动态变换**实现[^1][^2]。 2. **变换发布机制** 使用`tf::TransformBroadcaster`类向`/tf`话题发布坐标系间的变换信息。例如发布底盘到雷达的静态变换: ```python import tf from geometry_msgs.msg import TransformStamped broadcaster = tf.TransformBroadcaster() transform = TransformStamped() transform.header.stamp = rospy.Time.now() transform.header.frame_id = "base_link" # 父坐标系 transform.child_frame_id = "lidar" # 子坐标系 transform.transform.translation = (0.1, 0, 0.5) # 平移量 transform.transform.rotation = tf.transformations.quaternion_from_euler(0, 0, 0) # 旋转量 broadcaster.sendTransform(transform) ``` #### 二、坐标系转换方法 1. **监听与查询** 使用`tf.TransformListener`订阅`/tf`话题,通过`lookupTransform()`获取任意两个坐标系间的变换: ```python listener = tf.TransformListener() listener.waitForTransform("map", "lidar", rospy.Time(0), rospy.Duration(4.0)) (trans, rot) = listener.lookupTransform("map", "lidar", rospy.Time(0)) ``` 2. **数据类型转换** ROS提供多种几何类型转换函数[^3][^4]: - **欧拉角→四元数**:`quaternion_from_euler(roll, pitch, yaw)` - **四元数→旋转矩阵**:`quaternion_matrix([x,y,z,w])` - **坐标系变换矩阵合成**:`concatenate_matrices(trans_mat, rot_mat)` #### 三、关键注意事项 1. **时间同步** 查询变换时必须指定时间戳,若目标时间无对应坐标系数据会抛出`tf.ExtrapolationException`。建议使用`rospy.Time(0)`获取最新可用数据。 2. **TF树完整性** 所有相关坐标系必须通过变换连接成树状结构,若出现环路或断裂会导致查询失败。可通过`rqt_tf_tree`工具可视化检查。 3. **性能优化** 高频发布变换时建议使用`StaticTransformBroadcaster`发布静态变换,避免重复计算。 #### 四、典型应用场景 1. **多传感器融合** 将相机、雷达、IMU等传感器的数据统一转换到`map`坐标系下进行融合。 2. **导航定位** 通过`odom`→`base_link`的动态变换描述里程计信息,结合`map`→`odom`的校正实现定位[^2]。 3. **机械臂控制** 建立`base_link`→`arm_base`→`end_effector`的变换链,计算末端执行器位姿。 ```mermaid graph TD map --> odom odom --> base_link base_link --> lidar base_link --> camera base_link --> imu ```
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值