ROS tf廣播編寫

博客參考：https://www.ncnynl.com/archives/201702/1310.html

ROS與C++入門教程-tf-編寫tf broadcaster（廣播）

說明：

• 介紹如何廣播機器人的坐標系到tf。

准備：

• 在我們開始之前，你需要為這個項目創建一個新的ros包。
• 創建一個名為learning_tf的軟件包，該軟件包依賴於tf，roscpp，rospy和turtlesim：

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

$ cd ~/catkin_ws/src
$ catkin_make
$ source ./devel/setup.bash

廣播變換

• 新建文件turtle_tf_broadcaster.cpp. 參考源碼

$ roscd learning_tf
$ touch src/turtle_tf_broadcaster.cpp

• 代碼如下：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

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::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

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

代碼解釋：

• 代碼：#include <tf/transform_broadcaster.h>

• 作用：

  • tf包提供了TransformBroadcaster的實現，以幫助使發布變換的任務更容易。
  • 要使用TransformBroadcaster，我們需要包含tf/transform_broadcaster.h頭文件。

• 代碼：static tf::TransformBroadcaster br;

• 作用：我們創建一個TransformBroadcaster對象，我們稍后將使用它通過線路發送轉換。

• 代碼：

tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);

• 作用：這里我們創建一個Transform對象，並將信息從2D烏龜姿勢復制到3D變換中。

• 代碼：transform.setRotation(q);

• 作用：這里我們設置旋轉。

• 代碼：br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

• 作用：

  • 這是真正的工作完成的地方。
  • 使用TransformBroadcaster發送轉換需要四個參數。
  • 首先，我們傳遞轉換本身。
  • 現在我們需要給被發布的變換一個時間戳，我們只是用當前時間戳它，ros::Time::now().
  • 然后，我們需要傳遞我們創建的鏈接的父框架的名稱，在這種情況下為“world”
  • 最后，我們需要傳遞我們正在創建的鏈接的子框架的名稱，在這種情況下，這是烏龜本身的名稱。

• 注意：sendTransform和StampedTransform具有父對象和子對象的相反順序。

運行廣播

• 編輯CMakeLists.txt文件，增加：

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

• 編譯：

$ cd ~/catkin_ws/src
$ catkin_make

• 如果編譯順利， 生成二進制的turtle_tf_broadcaster文件，位於devel/lib/learning_tf

• 創建 start_demo.launch,用於啟動節點，內容如下：

<launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    <!-- Axes -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />

  </launch>

• 執行launch

$ roslaunch learning_tf start_demo.launch

• 你應該看到烏龜模擬另一個烏龜。

檢查結果：

• 現在，使用tf_echo工具檢查龜的姿勢是否真正得到廣播到tf

$ rosrun tf tf_echo /world /turtle1

• 這應該顯示第一只烏龜的姿勢。 使用箭頭鍵（確保您的終端窗口是活動的，而不是你的模擬器窗口）繞烏龜。
• 如果你運行tf_echo在/world和/turtle2之間的轉換，你不應該看到一個變換，因為第二個烏龜還沒有。
• 但是，一旦我們在下一個教程中添加第二只烏龜，turtle2的姿勢將被廣播到tf。