古月居ROS入门21讲——18.tf坐标系广播与监听的编程实现

1.如何实现一个tf广播器(broadcaster)

  1. 定义TF广播器(TransformBroadcaster)
  2. 创建坐标变换值
  3. 发布坐标变换(sendTransform)
#include 
#include 
#include 

std::string turtle_name;

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

    //初始化tf数据
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));  //Vector三维向量表示平移
    tf::Quaternion q;  //四元数表示旋转
    q.setRPY(0, 0, msg->theta);  //平面只有绕z轴的旋转
    transform.setRotation(q);

    //广播world与海龟坐标系之间的tf数据
    //tf::StampedTransform只能用在C++里,只是C++的一个类,一种数据格式,并不是一个消息。
    //将world和turtle_name的坐标转换关系发送出去
    broadcaster.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::NodeHandle n;
    ros::Subscriber sub= n.subscribe(turtle_name+"/pose", 10, &poseCallback);

    ros::spin();

    return 0;
}

2.如何实现一个tf监听器(listener)

  1. 定义TF监听器(TransformListener)
  2. 查找坐标变换(waitForTransform、lookupTransform)
#include 
#include 
#include 
#include 

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

    ros::NodeHandle n;

    //请求产生turtle2
    ros::service::waitForService("/spawn");  //等待发现/spawn服务
    ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("/spawn");  //创建一个服务客户端
    //初始化服务请求数据
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    //创建发布turtle2速度控制指令的发布者,发布名为/turtle2/cmd_vel的topic,消息类型为geometry_msgs::Twist,队列长度10
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

    //创建tf的监听器
    tf::TransformListener listener;

    ros::Rate loop_rate(10.0);

    while(n.ok())
    {
     
        //获取turtle1和turtle2坐标系之间的数据
        tf::StampedTransform transform;
        try
        {
     
            //等待系统中是否存在/turtle1、/turtle2这两个坐标系,存在的话才会跳到下一句,等待3秒之后就会提示超时,引发错误
            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;
        }

        //根据turtle1与turtle2坐标系之间的位置关系,计算turtle2的移动速度
        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());
        //发布turtle2的速度控制指令
        turtle_vel_pub.publish(vel_msg);

        loop_rate.sleep();
    }
    return 0;
}

3.配置CMakeLists.txt中的编译规则

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

4.编译与运行

cd ~/my_ws
catkin_make
source ~/my_ws/devel/setup.bash
roscore
rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
rosrun learning_tf turtle_tf_listener
rosrun turtlesim turtle_teleop_key

ROS重映射机制,第一次运行的tf广播__name:=turtle1_tf_broadcaster,程序中my_tf_broadcaster将被替换为turtle1_tf_broadcaster;第二次运行的tf广播__name:=turtle2_tf_broadcaster,程序中my_tf_broadcaster将被替换为turtle2_tf_broadcaster,同一个程序运行两次,避免命名产生冲突。

为什么要运行两次呢?这是因为每次广播的是小海龟和world坐标系之间的转换关系,然后在监听程序中获取查询turtle1和turtle2坐标系之间的转换关系,完成跟随。

古月居ROS入门21讲——18.tf坐标系广播与监听的编程实现_第1张图片

古月居ROS入门21讲——18.tf坐标系广播与监听的编程实现_第2张图片

参考:

  1. 古月居ROS入门21讲
  2. 中国大学MOOC—《机器人操作系统入门》课程讲义

你可能感兴趣的:(ROS)