ROS 機器人技術 - 靜態 TF 座標幀

上次總結了 ROS 機器人技術 - TF 座標系統基本概念，此次學習下如何發佈靜態座標轉換，靜態 TF 官方教程在這裏。

1、我在項目中應用 TF 的方法

先說下這個靜態座標轉換在我項目中的應用，我在建圖的時候須要一個 rslidar -> world 的 TF 變換，可是系統 TF 樹中只有師兄 lego_loam 的 base_link -> world的轉換，因此爲了獲取 rslidar -> world，我用 Kinetic 提供的工具手動發佈了一個靜態的 TF：

# 歐拉角
rosrun tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

# 四元數
rosrun tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

我用的第一種參數類型，由於我只是想要 rslidar -> world 的變換存在，不須要真實的旋轉和平移，因此我把 6 個轉換參數都設置爲 0：

rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link rslidar

這樣我就能在系統中找到 rslidar -> world 了，可是這句話是在命令行中運行的，每次測試都要運行一遍很麻煩，因此我把上面這句加到個人 launch 中，跟測試的節點一同啓動：

<launch>
  <!-- 其餘節點 -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="dlonng_static_transform_publisher" args="0 0 0 0 0 0 base_link rslidar" />
</launch>

以上就是這一節靜態的 TF 幫我完成的工做，下面就帶你手動實現一個靜態 TF 發佈程序，實際使用的時候不用本身寫，直接使用已經提供的 static_transform_publisher 便可。

2、本身編寫靜態 TF 發佈者

先在你的工做空間建立一個 learning_tf2 包，記得 source devel/setup.zsh ：

catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

而後進入這個包中，若是已經知道包名且環境變量也 source 了，能夠直接使用 roscd 進入，不用一個個輸入路徑：

roscd learning_tf2

如今編寫一個 static_turtle_tf2_broadcaster.cpp 放到 learning_tf2/src 目錄下，代碼以下：

#include <iostream>
#include <string>
#include <cstdio>

#include <ros/ros.h>

#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>

#include <geometry_msgs/TransformStamped.h>

std::string static_turtle_name;

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

    if(argc != 8) {
        ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw");
        return -1;
    }

    if(strcmp(argv[1], "world") == 0) {
        ROS_ERROR("Your static turtle name cannot be 'world'");
        return -1;
    }

    static_turtle_name = argv[1];

    // 建立 tf2 的廣播對象
    static tf2_ros::StaticTransformBroadcaster static_broadcaster;

    // 建立 tf2 要廣播的靜態座標變換
    geometry_msgs::TransformStamped static_transform_stamped;

    // 對座標變換初始化
    static_transform_stamped.header.stamp = ros::Time::now();
    // 父節點
    static_transform_stamped.header.frame_id = "camera";
    // 子節點
    static_transform_stamped.child_frame_id = static_turtle_name;

    // 初始化 x y z
    static_transform_stamped.transform.translation.x = atof(argv[2]);
    static_transform_stamped.transform.translation.y = atof(argv[3]);
    static_transform_stamped.transform.translation.z = atof(argv[4]);

    // 初始化四元數
    tf2::Quaternion quat;
    quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7]));

    static_transform_stamped.transform.rotation.x = quat.x();
    static_transform_stamped.transform.rotation.y = quat.y();
    static_transform_stamped.transform.rotation.z = quat.z();
    static_transform_stamped.transform.rotation.w = quat.w();

    // tf2 廣播對象發佈靜態座標變換
    static_broadcaster.sendTransform(static_transform_stamped);

    ROS_INFO("Spinning until killed publishing %s to world", static_turtle_name.c_str());

    ros::spin();
    return 0;
}

代碼很簡單，我也加了關鍵註釋，下面開始編譯，首先在 CMakeList.txt 中最末尾添加構建規則：

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

開始編譯：

catkin_make

基本不會遇到編譯錯誤，由於不是用 roslaunch 啓動，因此在運行以前須要先啓動 ros：

roscore

而後直接 rosrun 啓動這個節點：

$ rosrun learning_tf2 static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

注意這裏的參數沒有父節點，由於我在程序裏面指定父節點時 camera 了，而且參數的順序跟 ros 提供的 static_transform_publisher 不太同樣，不過不要緊，咱們實現的功能都是同樣的，運行後咱們能夠查看下這個變換是否發佈成功：

rostopic list

/tf_static

打印出這個變換的詳細參數，即平移和旋轉：

rostopic echo /tf_static

transforms:
  -
    header:
      seq: 0
      stamp:
        secs: 1459282870
        nsecs: 126883440
      frame_id: world
    child_frame_id: mystaticturtle
    transform:
      translation:
        x: 0.0
        y: 0.0
        z: 1.0
      rotation:
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---

若是你也能正確輸出相似的參數，那說明你發佈的變換也是 ok 的，後面能夠本身修改下平移和旋轉參數再編譯運行下基本就能搞定用法了。

3、強調一下下

上面的代碼只是爲了學習和理解 static_transform_publisher 原理用的，實際使用的時候仍是要用 ROS 提供的發佈工具，就是我文章開頭介紹的 2 種方法。

OK，今天就到這，下次見。