ROS 機器人技術 - 廣播與接收 TF 座標

登龍發表於2020-07-28
ROS機器人

上次我們學習了 TF 的基本概念和如何釋出靜態的 TF 座標：

這次來總結下如何釋出一個自定義的 TF 座標轉換，並監聽這個變換。

一、編寫 TF 廣播者

進入上次建立的 learning_tf2 包中：

roscd learning_tf2

在 src 下新建一個 turtle_tf2_broadcaster.cpp 檔案，程式碼如下：

#include <ros/ros.h>

// 儲存要釋出的座標變換
#include <geometry_msgs/TransformStamped.h>

// 四元數
#include <tf2/LinearMath/Quaternion.h>

// 變換廣播者
#include <tf2_ros/transform_broadcaster.h>

// 烏龜的位姿定義
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg) 
{
    // 建立 tf 廣播物件
    static tf2_ros::TransformBroadcaster br;

    // 儲存要釋出的座標變換訊息
    geometry_msgs::TransformStamped transformStamped;

    // 變換的時間戳
    transformStamped.header.stamp = ros::Time::now();
    
    // 父座標系名稱
    transformStamped.header.frame_id = "world";
    
    // 當前要釋出的座標系名稱 - 烏龜的名字
    transformStamped.child_frame_id = turtle_name;

    // 烏龜在二維平面運動，所以 z 座標高度為 0
    transformStamped.transform.translation.x = msg->x;
    transformStamped.transform.translation.y = msg->y;
    transformStamped.transform.translation.z = 0.0;

    // 用四元數儲存烏龜的旋轉角
    tf2::Quaternion q;

    // 因為烏龜在二維平面運動，只能繞 z 軸旋轉，所以 x，y 軸的旋轉量為 0
    q.setRPY(0, 0, msg->theta);

    // 把四元數拷貝到要釋出的座標變換中
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();
    
    // 用 tf 廣播者把訂閱的烏龜位姿釋出到 tf 中
    br.sendTransform(transformStamped);
}

int main(int argc, char** argv)
{
    // 當前節點的名稱
    ros::init(argc, argv, "my_tf2_broadcaster");
    ros::NodeHandle private_node("~");

    // 判斷當前要廣播的烏龜節點名字
    if (!private_node.hasParam("turtle")) {
        // launch 檔案和命令列都沒有傳遞烏龜名稱，就直接退出
        if (argc != 2) {
            ROS_ERROR("need turtle name as argument");
            return -1;
        };

        // launch 檔案中如果沒有定義烏龜名稱，就在命令列中加上
        turtle_name = argv[1];
    } else {
        // 從 launch 檔案獲取烏龜名稱引數
        private_node.getParam("turtle", turtle_name);
    }

    ros::NodeHandle node;

    // 訂閱一個節點的 pose msg，在回撥函式中廣播訂閱的位姿訊息到 tf2 座標系統中
    // turtle_name 為 turtle1 時廣播 turtle1 的位姿到 tf 中
    // turtle_name 為 turtle2 時廣播 turtle2 的位姿到 tf 中
    ros::Subscriber sub = node.subscribe(turtle_name + "/pose", 10, &poseCallback);

    ros::spin();
    return 0;
};

這個程式的意思是訂閱輸入烏龜的 pose 話題，然後在 poseCallback 回撥函式中釋出 world 到烏龜的 TF 變換，注意這個程式可以接收不同烏龜的 pose 訊息，只要執行時指定烏龜的名稱 turtle_name 即可，程式碼註釋很詳細，其他的就不說了，然後新增編譯規則：

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

直接編譯：

catkin_make

基本上不會出問題，為了方便啟動我們在 launch 檔案中啟動廣播者：

<launch>
     <!-- 烏龜節點 -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 控制烏龜運動的鍵盤節點 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <!-- 線速度和角速度的定義，但是在這個例子中並沒有用到哎... -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

    <!-- 第一個烏龜的 tf 廣播者節點，引數為烏龜 1 的名字 /tutle1 -->
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle1" name="turtle1_tf2_broadcaster" />
    
    <!-- 第二個烏龜的 tf 廣播者節點，還是用相同的節點，只不過改變了傳遞的引數為烏龜 2 的名字 /turtle2 --> 
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle2" name="turtle2_tf2_broadcaster" />

  </launch>

然後就可以直接啟動了：

roslaunch learning_tf2 start_demo.launch

為了確定是否成功廣播了變換，使用下面的命令檢視一個變換的輸出：

rosrun tf tf_echo /world /turtle1

如果在控制檯輸出類似下面的訊息，則說明變換髮布成功：

下面我們來編寫一個 TF 接收者來使用我們上面釋出的變換。

二、編寫 TF 接收者

同樣在 src 目錄下建立 turtle_tf2_listener.cpp，程式碼如下：

#include <ros/ros.h>

// 接受 tf 變換
#include <tf2_ros/transform_listener.h>

// 轉換訊息 
#include <geometry_msgs/TransformStamped.h>

// 釋出到烏龜 2 的運動訊息：角速度和線速度
#include <geometry_msgs/Twist.h>

// 再生服務
#include <turtlesim/Spawn.h>

// 實現烏龜 2 跟隨烏龜 1 運動
int main(int argc, char** argv)
{
    // 當前節點的名字
    ros::init(argc, argv, "my_tf2_listener");

    ros::NodeHandle node;
    ros::service::waitForService("spawn");
    ros::ServiceClient spawner = node.serviceClient<turtlesim::Spawn>("spawn");
    
    turtlesim::Spawn turtle;
    
    turtle.request.x = 4;
    turtle.request.y = 2;
    turtle.request.theta = 0;
    turtle.request.name = "turtle2";
    spawner.call(turtle);

    // 角速度和線速度訊息釋出者，用來發布計算後的新的速度訊息
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    // tf 變換快取區，最多快取 10 秒
    tf2_ros::Buffer tfBuffer;

    // 建立監聽 tf 變換物件，建立完畢即開始監聽，通常定義為成員變數
    tf2_ros::TransformListener tfListener(tfBuffer);
    
    ros::Rate rate(10.0);
    while (node.ok()) {
        // 用來儲存尋找的座標變換
        geometry_msgs::TransformStamped transformStamped;
        try{
          	// 尋找座標變換
      		  transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
        }
        catch (tf2::TransformException &ex) {
            ROS_WARN("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        // 用來儲存角速度和線速度
        geometry_msgs::Twist vel_msg;

        // 新的角速度為尋找到的變換角速度的 4 倍 - 使得第二個烏龜的運動軌跡轉彎更快，且軌跡是弧線
        vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x);
        
        // 新的線速度是尋找到的變換線速度的 0.5 倍 - 使得第二個烏龜的運動速度為第一個烏龜的一半
        vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2));
        
        // 釋出新的速度訊息，烏龜 2 節點的內部訂閱了這個訊息，所以烏龜 2 會收到新的角速度和線速度，以此產生跟隨運動
        turtle_vel.publish(vel_msg);
      
        rate.sleep();
    }

    return 0;
};

這裡關鍵的程式碼如下：

// 儲存尋找的變換
geometry_msgs::TransformStamped transformStamped;

// 尋找 turtle1 到 turtle2 的座標變換
// target_frame: turtle2 
// source_frame: turtle1
// ros::Time(0): 獲取變換的時間，
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

同樣新增編譯規則：

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

然後編譯：

catkin_make

在上面廣播者的 launch 檔案中加上接收者的啟動：

<!-- 
  這個例子一共建立了 5 個節點：
    1. 烏龜節點，包含 2 個小烏龜
    2. 控制烏龜運動的鍵盤節點
    3. 第一個烏龜的 tf 廣播者節點
    4. 第二個烏龜的 tf 廣播者節點
    5. tf 座標系統的監聽節點，用來監聽 2 個烏龜之間的座標變換
-->
<launch>
     <!-- 烏龜節點，這個節點的內部應該是建立了 2 個烏龜...... -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 控制烏龜運動的鍵盤節點 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <!-- 線速度和角速度的定義，但是在這個例子中並沒有用到哎... -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

    <!-- 第一個烏龜的 tf 廣播者節點，引數為烏龜 1 的名字 /tutle1 -->
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle1" name="turtle1_tf2_broadcaster" />
    
    <!-- 第二個烏龜的 tf 廣播者節點，還是用相同的節點，只不過改變了傳遞的引數為烏龜 2 的名字 /turtle2 --> 
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle2" name="turtle2_tf2_broadcaster" />

    <!-- 啟動 tf 座標系同的監聽節點 -->
    <node pkg="learning_tf2" type="turtle_tf2_listener" name="listener" />

  </launch>

然後啟動：

roslaunch learning_tf2 start_demo.launch

執行時會出現 2 個小烏龜，把視窗焦點放到終端，按上下左右鍵會發現第二個烏龜跟隨第一個烏龜運動：

但是剛啟動時終端會報個錯誤：

[ERROR] [1418082761.220546623]: "turtle2" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1418082761.320422000]: "turtle2" passed to lookupTransform argument target_frame does not exist.

這是因為我們在 turtle2 還沒有產生之前就尋找變換，導致沒有找到它，為了解決這個問題可以在尋找變換前等待變換可用：

// 第四個引數是阻塞等待的超時時間
listener.waitForTransform("/turtle2", "/turtle1", ros::Time::now(), ros::Duration(3.0));

transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

加上這句執行時就不會報錯了，今天就寫到這裡，下次見：）

相關文章