TF2动态转换-dynamic

最新推荐文章于 2024-05-29 16:54:03 发布

Mr_rustylake

最新推荐文章于 2024-05-29 16:54:03 发布

阅读量179

点赞数

分类专栏： ros 文章标签： c++ 机器人学习方法

本文链接：https://blog.csdn.net/Mr_rustylake/article/details/128508384

版权

ros 专栏收录该内容

32 篇文章 2 订阅

订阅专栏

但不同的坐标系可能不像是之前静态的pub,发布方的坐标是动态变化的，所以我们不能时时改写发布方的文档来适应它，这不合理也不精准。于是我们引出动态转换的方法。

我们首先要rosrun一个动态的发布方，以turtlesim为例。我们进行rosrun turtlesim turtlesim_node和rosrun turtlesim turtle_teleop_key。

然后我们就编写发布方文件：

#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& pose){
    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=pose->x;
    ts.transform.translation.y=pose->y;
    ts.transform.translation.z=0.0;

    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->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;
}

这些函数和参数的解析见上一篇关于静态转换的交流文字。注意修改坐标系名称。

接下来我们编写订阅方的文档：

#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,"dynamic_sub");
    ros::NodeHandle n;

    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    geometry_msgs::PointStamped ps;
    ps.header.frame_id="turtle1";
    ps.header.stamp=ros::Time(0.0);
    ps.point.x=2.0;
    ps.point.y=3.0;
    ps.point.z=5.0;

    //ros::Duration(1).sleep();
    ros::Rate rate(10);

    while(ros::ok()){
        geometry_msgs::PointStamped ps_out;
        
        try{
            ps_out=buffer.transform(ps,"world");
            ROS_INFO("transformed location:( %.2f , %.2f , %.2f ), the reference frame: %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("exception message: %s",e.what());
        }

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

    return 0;
}

其实这个文档和上次静态转换的订阅方差不多，要修改的是：

1.一个是时间戳需要修改为ros::Time(0.0)，因为当我们设置为现在时间的时候，因为不同对象在电脑在创立时是有时间差的，虽然它可能很小，但是对于电脑来说，这已经是比较大的误差。

2.修改坐标系名称。

最后进行发布可以得到。