多坐标变换

最新推荐文章于 2024-03-11 14:10:29 发布

三个刺客

最新推荐文章于 2024-03-11 14:10:29 发布

阅读量159

点赞数 1

分类专栏： ROS 文章标签：自动驾驶 linux

本文链接：https://blog.csdn.net/mxh3600/article/details/125333463

版权

ROS 专栏收录该内容

23 篇文章 12 订阅

订阅专栏

需求描述:

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

实现分析:

首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
最后，还要实现坐标点的转换

实现流程:C++ 与 Python 实现流程一致

新建功能包，添加依赖
创建坐标相对关系发布方(需要发布两个坐标相对关系)
创建坐标相对关系订阅方
执行

1.创建功能包 tf03_tfs

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方 tfs_c.launch

为了方便，使用静态坐标变换发布也即是ROS底层的程序，类似于turtlesim turtlesim_node

<launch>
      <!-- 发布son1相对于world以及son2相对于world的坐标关系 -->
      <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>

其中ags是son1相当于world的欧拉角与偏移量

启动：

roslaunch tf03_tfs tfs_c.launch

rviz

3.订阅方

#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"
#include "geometry_msgs/TransformStamped.h"

/*
     订阅方实现：1计算son1与son2的相对关系 2计算son1的中某个坐标点再son2中的坐标值
*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;
    
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    
    // 创建坐标点
    geometry_msgs::PointStamped psAtSon1;
    
    psAtSon1.header.stamp = ros::Time::now();
    psAtSon1.header.frame_id = "son1";
    psAtSon1.point.x = 1.0;
    psAtSon1.point.y = 2.0;
    psAtSon1.point.z = 3.0;

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            //1计算son1与son2的相对关系
            /*
            A相对于B的坐标系关系

            参数1：目标坐标系  B
            参数2：源坐标系    A
            参数3：ros::Time(0) 取间隔最短的两个坐标关键帧计算相对关系
            返回值：geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系

            */
            geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("son1","son2",ros::Time(0));
            ROS_INFO("son1相对于son2的信息:父级:%s,子级:%s 偏移量(%.2f,%.2f,%.2f) ",
                      son1ToSon2.header.frame_id.c_str(),
                      son1ToSon2.child_frame_id.c_str(),
                      son1ToSon2.transform.translation.x,
                      son1ToSon2.transform.translation.y,
                      son1ToSon2.transform.translation.z
                      );
            //2计算son1的中某个坐标点再son2中的坐标值
            geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1,"son2");
            ROS_INFO("坐标点再son2中的坐标值(%.2f,%.2f,%.2f)",
                      psAtSon2.point.x,
                      psAtSon2.point.y,
                      psAtSon2.point.z
                      );
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示：%s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}