ROS tf坐标系广播与监听的编程实现

最新推荐文章于 2024-09-21 14:28:01 发布

原创

最新推荐文章于 2024-09-21 14:28:01 发布 · 300 阅读

0 ·

CC 4.0 BY-SA版权

创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

====================

如何实现一个tf广播器

定义TF广播器（TransformBroadcaster）
创建坐标变换值
发布坐标变换（sendTransform）

turtle_tf_broadcaster.cpp

//创建tf广播器代码（C++）

/**
* 该例程产生tf数据，并计算、发布turtle2的速度指令
*/
#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    // 创建tf 的广播器
    static tf::TransformBroadcaster br;

    //初始化tf数据
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x,msg->y,0.0));
    tf::Quaternion q;
    q.setRPY(0,0,msg->theta);
    transform.setRotation(q);

    // 广播world与海龟坐标系之间的tf数据
    br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));
}

int main(int argc,char **argv)
{
    // 初始化ROS节点