ROS（3）广播模型

 广播模型

广播器

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

CMakeList的find_package中增加tf

turtle_tf_broadcaster.cpp

// 订阅海龟话题，获取pose位置，之后创建tf广播其，广播world与海龟坐标系之间的tf数据

#include 
#include   // 小海龟位置
#include   // 位置广播

std::string turtle_name;


void poseCallBack(const turtlesim::PoseConstPtr &msg)
{
    static tf::TransformBroadcaster br;

    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);

    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_tf_broadcaster");

    if(argc != 2)
    {
        ROS_ERROR("need turtle name as argument");
        return -1;
    }

    turtle_name = argv[1];
    ROS_INFO("turtle name is %s", turtle_name.c_str());

    // 构建订阅者，订阅海龟的/pose话题
    ros::NodeHandle n;
    ros::Subscriber subs = n.subscribe(turtle_name + "/pose", 10, &poseCallBack);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

监听器

定义TF监听器（TransformListener）

查找坐标变换（waitForTransform、lookupTransform）

turtle_tf_listener.cpp

#include 
#include 
#include 
#include   // 位置广播


int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_tf_listener");

    ros::NodeHandle n;

    //等待服务端服务
    ros::service::waitForService("/spawn");
    // 创建客户端，连接服务为/spawn，数据类型为turtlesim::Spawn
    ros::ServiceClient add_turtle = n.serviceClient("/spawn");
    // 调用服务，请求完成后生成小海龟
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    // 构建发布者，发布话题 /turtle2/cmd_vel，话题类型为geometry_msgs::Twist
    ros::Publisher turtle_vel = n.advertise("/turtle2/cmd_vel", 10);

    // 构建监听者
    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (ros::ok)
    {
        tf::StampedTransform transform;
        try
        {
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        }
        catch(tf::TransformException &ex)
        {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
        turtle_vel.publish(vel_msg);

        rate.sleep();
    }

    return 0;
}

执行命令

roscore

rosrun turtlesim turtlesim_node

rosrun 功能包 turtle_tf_broadcaster节点名 __name:=turtle1_tf_broadcaster /turtle1

rosrun 功能包 turtle_tf_broadcaster节点名 __name:=turtle2_tf_broadcaster /turtle2

rosrun 功能包 turtle_tf_listener

rosrun turtlesim turtle_teleop_key