我的ROS学习之路——多坐标变换

2021-11-26 22:34:49 阅读：250 来源： 互联网

原文请点击。

需求描述:

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

实现分析:

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

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

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

1.创建功能包

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

2.发布方

为了方便，使用静态坐标变换发布（直接使用内置的命令发布两个静态坐标系，就不用代码实现了，在前面讲的静态坐标变换中也讲了如何发布）

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" />
</launch>

3.订阅方

#include <ros/ros.h>
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2/LinearMath/Transform.h"
#include "tf2/LinearMath/Quaternion.h"


int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"transform_point");
    ros::NodeHandle nh;
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    //设置被转换的坐标点
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "son1";
    ps.header.stamp = ros::Time(0);
    ps.point.x = 2;//数据写的规整一点，方便观察灯会结果对不对
    ps.point.y = 0;
    ps.point.z = 0;


    ros::Rate rate(1);
while(ros::ok())
{

    try
    {
        geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));//使用重载函数一，有三个参数：目标坐标系A（被参考的那个），源坐标系B，时间。前两个参数的意思是：B相对于A 变化了多少，
        //就可以从lookupTransform这个函数计算出来。就类似于 广播 transform，自己设置的 偏移量和欧拉角一样。这里只是反求了而已。
            ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());
            ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());
            ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",
                    tfs.transform.translation.x,
                    tfs.transform.translation.y,
                    tfs.transform.translation.z
                    );                       

        geometry_msgs::PointStamped ps_out =  buffer.transform(ps,"son2");//这个目标坐标系，可以填 world 也可以填son2都可以转换成功。
        ROS_INFO("转换后的坐标系为（%.2f,%.2f,%.2f）,参考的坐标系为：%s",
        ps_out.point.x,
        ps_out.point.y,
        ps_out.point.z,
        ps_out.header.frame_id.c_str()
    );
    }
    catch(const std::exception& e)
    {
       ROS_INFO("程序异常...",e.what());
    }
    rate.sleep();
}
    return 0;
}

配置文件此处略。

4.执行

（可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，将输出换算后的结果。）

我这里是先启动launch文件，再启动dynamic_more_tf_sub节点。

结果如图：

结束~

标签：ps,ROS,msgs,变换,son2,坐标,ros,tf2
来源： https://blog.csdn.net/qq_42537872/article/details/121569683

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9

我的ROS学习之路——多坐标变换