座標變換TF
座標變換
引言與應用場景
里程计ODOM
惯性计IMU
激光雷达Laser
摄像头Camera
場景1：現有一移動式機器人底盤，在底盤上安裝了一雷達，雷達相對於底盤的偏移量已知，現雷達檢測到一障礙物信息，獲取到座標分別為(x,y,z)，該座標是以雷達為參考系的，如何將這個座標轉換成以小車為參考系的座標呢？
激光雷達與小車的中心或邊緣相差的橫縱距離，以及激光雷達與牆的距離及小車與牆的距離。
場景2:現有一帶機械臂的機器人(比如:PR2)需要夾取目標物，當前機器人頭部攝像頭可以探測到目標物的座標(x,y,z)，不過該座標是以攝像頭為參考系的，而實際操作目標物的是機械臂的夾具，當前我們需要將該座標轉換成相對於機械臂夾具的座標，這個過程如何實現？
以上通過TF即可算
概念與作用
TF實行右手座標系
重要的就是相對位置和時間，在某個時間某個物體位於某個位置。（時間差太大，數據會被廢棄）
案例安裝以及運行
安裝烏龜案列：
# Humble版本安装
sudo apt-get install ros-humble-turtle-tf2-py ros-humble-tf2-tools ros-humble-tf-transformations
# Jazzy版本安装
sudo apt-get install ros-jazzy-turtle-tf2-py ros-jazzy-tf2-tools ros-jazzy-tf-transformations
此外，還需要安裝一個名為 transforms3d 的 Python 包，它為 tf_transformations包提供四元數和歐拉角變換功能，安裝命令如下：
# 方式一（不推荐）
sudo apt intall python3-pip
pip3 install transforms3d
#方式二（推荐）
sudo apt install python3-transforms3d
啟動兩個終端，終端1輸入如下命令：
ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py
該命令會啟動 turtlesim_node 節點，turtlesim_node 節點中自帶一隻小烏龜 turtle1，除此之外還會新生成一隻烏龜 turtle2，turtle2 會運行至 turtle1 的位置。
終端2輸入如下命令：
ros2 run turtlesim turtle_teleop_key
該終端下可以通過鍵盤控制 turtle1 運動，並且 turtle2 會跟隨 turtle1 運動（參考引言部分的 案例1 ）。
龜男🐢🚹會跟隨前面的烏龜🐢運動
座標變換相關消息
座標變換的實現其本質是基於話題通信的發佈訂閱模型的，發佈方可以發佈座標系之間的相對關係，訂閱方則可以監聽這些消息，並實現不同座標系之間的變換。顯然的根據之前介紹，在話題通信中，接口消息作為數據載體在整個通信模型中是比較重要的一部分，本節將會介紹座標變換中常用的兩種接口消息：geometry_msgs/msg/TransformStamped和geometry_msgs/msg/PointStamped。
前者用於描述某一時刻兩個座標系之間相對關係的接口，後者用於描述某一時刻座標系內某個座標點的位置的接口。在座標變換中，會經常性的使用到座標系相對關係以及座標點信息。
geometry_msgs/msg/TransformStamped
通過如下命令查看接口定義：
ros2 interface show geometry_msgs/msg/TransformStamped
接口定義解釋：
std_msgs/Header header
builtin_interfaces/Time stamp # 时间戳
int32 sec #秒
uint32 nanosec #纳秒
string frame_id # 父级坐标系
string child_frame_id # 子级坐标系
Transform transform # 子级坐标系相对于父级坐标系的位姿
Vector3 translation # 三维偏移量
float64 x
float64 y
float64 z
Quaternion rotation # 四元数
float64 x 0
float64 y 0
float64 z 0
float64 w 1
描述一個物體運動一般有6個自由度：X，Y，Z，Yaw，Pitch，Roll。
三個平動，三個旋轉：
Vector3 translation代表3個平移
Quaternion rotation四元數可以轉化為三個歐拉角(yaw，pitch，roll)
（Q：為何不用歐拉角而用四元數？A：因為用歐拉角計算會出現死鎖現象，所以選擇用四元數，而不用歐拉角，以便避免歐拉角的缺陷。）
3個平移以米meter為單位
3個旋轉以弧度rad為單位
四元數類似於歐拉角用於表示座標系的相對姿態，
具體轉化詳見大疆開發板C型嵌入式軟件教程文檔.pdf的18.3節
具體轉化算法（Mahony算法）（ROS2的TF2庫中也有具體的轉化算法）：
https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
按右手座標系來看，N2相對於N1沿X軸平移了1m(一格代表1米）。
旋轉
geometry_msgs/msg/PointStamped
通過如下命令查看接口定義：
ros2 interface show geometry_msgs/msg/PointStamped
接口定義解釋：
std_msgs/Header header
builtin_interfaces/Time stamp # 时间戳
int32 sec #秒
uint32 nanosec #纳秒
string frame_id # 参考系
Point point # 三维坐标
float64 x
float64 y
float64 z
在三維中的座標點
座標變換廣播
引言與案例及分析
座標系相對關係主要有兩種： 靜態座標系相對關係 與 動態座標系相對關係 。
所謂靜態座標系相對關係是指兩個座標系之間的相對位置是固定不變的，比如：車輛上的雷達、攝像頭等組件一般是固定式的，那麼雷達座標系相對於車輛底盤座標系或攝像頭座標系相對於車輛底盤座標系就是一種靜態關係。
所謂動態座標系相對關係是指兩個座標系之間的相對位置關係是動態改變的，比如：車輛上機械臂的關節或夾爪、多車編隊中不同車輛等都是可以運動的，那麼機械臂的關節或夾爪座標系相對車輛底盤座標系或不同車輛座標系的相對關係就是一種動態關係。
本節會主要介紹如何實現靜態座標變換廣播與動態座標變換廣播。另外，本節還會演示如何發佈座標點消息。
激光雷達Laser和色相頭與底盤位置是靜態的，而機械臂的末端執行器與機器人的位置是動態的。
1.案例需求
案例1： 現有一無人車，在無人車底盤上裝有固定式的雷達與攝像頭，已知車輛底盤、雷達與攝像頭各對應一座標系，各座標系的原點取其幾何中心。現又已知雷達座標系相對於底盤座標系的三維平移量分別為：x方向0.4米，y方向0米，z方向0.2米，無旋轉。攝像頭座標系相對於底盤座標系的三維平移量分別為：x方向-0.5米，y方向0米，z方向0.4米，無旋轉。請廣播雷達與底盤的座標系相對關係，攝像頭與底盤的座標系相對關係，並在 rviz2 中查看廣播的結果。
案例2： 啟動 turtlesim_node，設該節點中窗體有一個世界座標系(左下角為座標系原點)，烏龜是另一個座標系，烏龜可以通過鍵盤控制運動，請動態發佈烏龜座標系與世界座標系的相對關係。
2.案例分析
在上述案例中，案例1需要使用到靜態座標變換，案例2則需要使用動態座標變換，不論無論何種實現關注的要素都有兩個：
- 如何廣播座標系相對關係；
- 如何使用 rviz2 顯示座標系相對關係。
3.流程簡介
以編碼的實現實現靜態或動態座標變換的流程類似，主要步驟如下：
- 編寫廣播實現；
- 編輯配置文件；
- 編譯；
- 執行；
- 在 rviz2 中查看座標系關係。
案例我們會採用 C++ 和 Python 分別實現，二者都遵循上述實現流程。
另外：需要說明的是，靜態廣播器除了可以以編碼的方式實現外，在 tf2 中還內置了相關工具，可以無需編碼，直接執行節點並傳入表示座標系相對關係的參數，即可實現靜態座標系關係的發佈（即命令行，有命令行可以優先用命令行）。而動態廣播器沒有提供類似的工具。（即必須敲代碼）
4.準備工作
終端下進入工作空間的src目錄，調用如下兩條命令分別創建C++功能包。
ros2 pkg create cpp03_tf_broadcaster --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs turtlesim
tf2功能包內包含了四元數與歐拉角的轉換算法。
tf2_ros功能包內包含了廣播對象。
geometry_msgs功能包消息載體
turtlesim功能包是獲取烏龜🐢位姿
靜態廣播器_命令行實現
1.靜態廣播器工具
在 tf2_ros功能包中提供了一個名為static_transform_publisher的可執行文件，通過該文件可以直接廣播靜態座標系關係，其使用語法如下。
格式1：
使用以米為單位的 x/y/z 偏移量和以弧度為單位的roll/pitch/yaw（可直譯為滾動/俯仰/偏航，分別指的是圍繞 x/y/z 軸的旋轉）向 tf2 發佈靜態座標變換。
ros2 run tf2_ros static_transform_publisher --x x --y y --z z --yaw yaw --pitch pitch --roll roll --frame-id frame_id --child-frame-id child_frame_id
父級座標系和子級座標系是必須要寫的。
如果這些可選參數不選的話，默認父級座標系和子級座標系重合，也就是偏移量和旋轉度都是0。
偏移量：X,Y,Z
旋轉度：QX,QY,QZ,QW 或者 ROLL,PITCH,YAW（單位是弧度）
時間戳：不用設置，會以發佈的時間為起點
格式2：
使用以米為單位的 x/y/z 偏移量和 qx/qy/qz/qw 四元數向 tf2 發佈靜態座標變換。
ros2 run tf2_ros static_transform_publisher --x x --y y --z z --qx qx --qy qy --qz qz --qw qw --frame-id frame_id --child-frame-id child_frame_id
注意：在上述兩種格式中除了用於表示父級座標系的--frame-id和用於表示子級座標系的--child-frame-id之外，其他參數都是可選的，如果未指定特定選項，那麼將直接使用默認值。
2.靜態廣播器工具使用
打開兩個終端，終端1輸入如下命令發佈雷達（laser）相對於底盤（base_link）的靜態座標變換（重合）：
ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser
base_link是父級參考系
laser是子級參考系
一般選父系參考系
3.rviz2 查看座標系關係
新建終端，通過命令rviz2打開 rviz2 並配置相關插件查看座標變換消息：
- 將 Global Options 中的 Fixed Frame 設置為 base_link；
- 點擊 add 按鈕添加 TF 插件；
- 勾選 TF 插件中的 show names。
右側 Grid 中將以圖形化的方式顯示座標變換關係。
如圖，兩個參考系重合。
打開兩個終端，終端1輸入如下命令發佈雷達（laser）相對於底盤（base_link）的靜態座標變換：
ros2 run tf2_ros static_transform_publisher --x 0.4 --y 0 --z 0.2 --yaw 0.5 --roll 0 --pitch 0 --frame-id base_link --child-frame-id laser
終端2輸入如下命令發佈攝像頭（camera）相對於底盤（base_link）的靜態座標變換：
ros2 run tf2_ros static_transform_publisher --x -0.5 --y 0 --z 0.4 --yaw 0 --roll 0 --pitch 0 --frame-id base_link --child-frame-id camera
靜態廣播器_C++實現
框架搭建
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)
add_executable(demo01_static_tf_broadcaster src/demo01_static_tf_broadcaster.cpp)
ament_target_dependencies(
demo01_static_tf_broadcaster
"rclcpp"
"tf2"
"tf2_ros"
"geometry_msgs"
"turtlesim"
)
install(TARGETS demo01_static_tf_broadcaster
DESTINATION lib/${PROJECT_NAME})
廣播實現
/*
需求：编写静态坐标变换程序，执行时传入两个坐标系的相对位姿关系以及父子级坐标系id，
程序运行发布静态坐标变换。
步骤：
1.包含头文件；
2.判断终端传入的参数是否合法；
3.初始化 ROS 客户端；
4.定义节点类；
4-1.创建静态坐标变换发布方；
4-2.组织并发布消息。
5.调用 spin 函数，并传入对象指针；
6.释放资源。
*/
// 1.包含头文件；
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>
using std::placeholders::_1;
// 4.定义节点类；
class MinimalStaticFrameBroadcaster : public rclcpp::Node
{
public:
explicit MinimalStaticFrameBroadcaster(char * transformation[]): Node("minimal_static_frame_broadcaster")
{
// 4-1.创建静态坐标变换发布方；
tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
this->make_transforms(transformation);
}
private:
// 4-2.组织并发布消息。
void make_transforms(char * transformation[])
{
// 组织消息
geometry_msgs::msg::TransformStamped t;
rclcpp::Time now = this->get_clock()->now();
t.header.stamp = now;
t.header.frame_id = transformation[7];
t.child_frame_id = transformation[8];
t.transform.translation.x = atof(transformation[1]);
t.transform.translation.y = atof(transformation[2]);
t.transform.translation.z = atof(transformation[3]);
tf2::Quaternion q;
q.setRPY(
atof(transformation[4]),
atof(transformation[5]),
atof(transformation[6]));
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();
// 发布消息
tf_publisher_->sendTransform(t);
}
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_;
};
int main(int argc, char * argv[])
{
// 2.判断终端传入的参数是否合法；
auto logger = rclcpp::get_logger("logger");
if (argc != 9) {
RCLCPP_INFO(
logger, "运行程序时请按照：x y z roll pitch yaw frame_id child_frame_id 的格式传入参数");
return 1;
}
// 3.初始化 ROS 客户端；
rclcpp::init(argc, argv);
// 5.调用 spin 函数，并传入对象指针；
rclcpp::spin(std::make_shared<MinimalStaticFrameBroadcaster>(argv));
// 6.释放资源。
rclcpp::shutdown();
return 0;
}
必須傳參正確，否則拋錯。
創建組織併發布數據的函數
用最簡單的重載。
這樣就可以發送了，接下來編輯發送的內容。
stamp時間戳設置為當前時間，now()函數是設置為當前時間。
atof()是轉化為float浮點類型
x()和getx()都是獲取四元數x。（y，z，w以此類推）
終端中進入當前工作空間，編譯功能包：
colcon build --packages-select cpp03_tf_broadcaster
當前工作空間下，啟動兩個終端，終端1輸入如下命令發佈雷達（laser）相對於底盤（base_link）的靜態座標變換：
. install/setup.bash
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0 0.2 0 0 0 base_link laser
參考 靜態廣播器（命令） 內容啟動並配置 rviz2，最終執行結果與案例1類似。
本質就是話題通信，但這個話題通信的topic是啥呢？
通過查看該類源碼，就得知，話題為/tf_static
發佈方有倆，訂閱方有一個。
這倆是發佈方
這個是訂閱方
動態廣播器_C++實現
框架搭建
CMakeLists.txt 文件需要添加如下內容：
add_executable(demo02_dynamic_tf_broadcaster src/demo02_dynamic_tf_broadcaster.cpp)
ament_target_dependencies(
demo02_dynamic_tf_broadcaster
"rclcpp"
"tf2"
"tf2_ros"
"geometry_msgs"
"turtlesim"
)
文件中 install 修改為如下內容：
install(TARGETS demo01_static_tf_broadcaster
demo02_dynamic_tf_broadcaster
DESTINATION lib/${PROJECT_NAME})
廣播實現
/*
需求：编写动态坐标变换程序，启动 turtlesim_node 以及 turtle_teleop_key 后，该程序可以发布
乌龟坐标系到窗口坐标系的坐标变换，并且键盘控制乌龟运动时，乌龟坐标系与窗口坐标系的相对关系
也会实时更新。
步骤：
1.包含头文件；
2.初始化 ROS 客户端；
3.定义节点类；
3-1.创建动态坐标变换发布方；
3-2.创建乌龟位姿订阅方；
3-3.根据订阅到的乌龟位姿生成坐标帧并广播。
4.调用 spin 函数，并传入对象指针；
5.释放资源。
*/
// 1.包含头文件；
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <turtlesim/msg/pose.hpp>
using std::placeholders::_1;
// 3.定义节点类；
class MinimalDynamicFrameBroadcaster : public rclcpp::Node
{
public:
MinimalDynamicFrameBroadcaster(): Node("minimal_dynamic_frame_broadcaster")
{
// 3-1.创建动态坐标变换发布方；
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
std::string topic_name = "/turtle1/pose";
// 3-2.创建乌龟位姿订阅方；
subscription_ = this->create_subscription<turtlesim::msg::Pose>(
topic_name, 10,
std::bind(&MinimalDynamicFrameBroadcaster::handle_turtle_pose, this, _1));
}
private:
// 3-3.根据订阅到的乌龟位姿生成坐标帧并广播。
void handle_turtle_pose(const turtlesim::msg::Pose & msg)
{
// 组织消息
geometry_msgs::msg::TransformStamped t;
rclcpp::Time now = this->get_clock()->now();
t.header.stamp = now;
t.header.frame_id = "world"; //窗体坐标系
t.child_frame_id = "turtle1"; //乌龟坐标系
t.transform.translation.x = msg.x;
t.transform.translation.y = msg.y;
t.transform.translation.z = 0.0; //乌龟在平面内运动
//从欧拉角转换为四元数
tf2::Quaternion q;
q.setRPY(0, 0, msg.theta); //乌龟只有Yaw
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();
// 发布消息
tf_broadcaster_->sendTransform(t);
}
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};
int main(int argc, char * argv[])
{
// 2.初始化 ROS 客户端；
rclcpp::init(argc, argv);
// 4.调用 spin 函数，并传入对象指针；
rclcpp::spin(std::make_shared<MinimalDynamicFrameBroadcaster>());
// 5.释放资源。
rclcpp::shutdown();
return 0;
}
入口參數：
參數1話題名稱
參數2QoS
參數3回調函數
操控烏龜運動，會使烏龜在Rviz2裡也運動
座標點發布_C++實現
案例與分析
案例需求
案例： 無人車上安裝有激光雷達，現激光雷達掃描到一點狀障礙物並且可以定位障礙物的座標，請在雷達座標系下發布障礙物座標點數據，並在 rviz2 中查看發佈結果。
案例分析
上述案例，是一個簡單的話題發佈程序，在瞭解座標點geometry_msgs/msg/PointStamped接口消息之後，直接通過話題發佈方按照一定邏輯發佈消息即可。
流程簡介
程序實現主要步驟如下：
- 編寫話題發佈實現；
- 編輯配置文件；
- 編譯；
- 執行；
- 在 rviz2 中查看運行結果。
案例我們會採用 C++ 和 Python 分別實現，二者都遵循上述實現流程。
CMakeLists.txt 文件需要添加如下內容：
add_executable(demo03_point_publisher src/demo03_point_publisher.cpp)
ament_target_dependencies(
demo03_point_publisher
"rclcpp"
"tf2"
"tf2_ros"
"geometry_msgs"
"turtlesim"
)
文件中 install 修改為如下內容：
install(TARGETS demo01_static_tf_broadcaster
demo02_dynamic_tf_broadcaster
demo03_point_publisher
DESTINATION lib/${PROJECT_NAME})
實現
/*
需求：发布雷达坐标系中某个坐标点相对于雷达（laser）坐标系的位姿。
步骤：
1.包含头文件；
2.初始化 ROS 客户端；
3.定义节点类；
3-1.创建坐标点发布方；
3-2.创建定时器；
3-3.组织并发布坐标点消息。
4.调用 spin 函数，并传入对象指针；
5.释放资源。
*/
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
using namespace std::chrono_literals;
// 3.定义节点类；
class MinimalPointPublisher: public rclcpp::Node {
public:
MinimalPointPublisher(): Node("minimal_point_publisher"),x(0.1){
// 3-1.创建坐标点发布方；
point_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>("point",10);
// 3-2.创建定时器；
timer_ = this->create_wall_timer(0.1s,std::bind(&MinimalPointPublisher::on_timer, this));
}
private:
void on_timer(){
// 3-3.组织并发布坐标点消息。
geometry_msgs::msg::PointStamped point;
point.header.frame_id = "laser";
point.header.stamp = this->now();
x += 0.004;
point.point.x = x;
point.point.y = 0.0;
point.point.z = 0.1;
point_pub_->publish(point);
}
rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr point_pub_;
rclcpp::TimerBase::SharedPtr timer_;
double_t x;
};
int main(int argc, char const *argv[])
{
// 2.初始化 ROS 客户端；
rclcpp::init(argc,argv);
// 4.调用 spin 函数，并传入对象指针；
rclcpp::spin(std::make_shared<MinimalPointPublisher>());
// 5.释放资源。
rclcpp::shutdown();
return 0;
}
創建定時器用create_wall_time()函數，要填時間間隔和對應的回調函數。
執行
當前工作空間下，啟動兩個終端，終端1輸入如下命令發佈雷達（laser）相對於底盤（base_link）的靜態座標變換：
. install/setup.bash
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0 0.2 0 0 0 base_link laser
終端2輸入如下命令發佈障礙物相對於雷達（laser）的座標點：
. install/setup.bash
ros2 run cpp03_tf_broadcaster demo03_point_publisher
rviz2 查看座標系關係
參考 5.3.2 靜態廣播器（命令） 內容啟動並配置 rviz2，顯示座標變換後，再添加 PointStamped 插件並將其話題設置為 /point，最終顯示結果與案例演示類似。
通過這裡可以改球的透明度和大小
小結
靜態廣播只發布一次。
而動態廣播和座標點廣播都是發佈多次。
但實質上就是話題通信。
座標變換監聽
案例與分析
案例1： 在 5.3 座標變換廣播 中發佈了laser相對於base_link和camra相對於base_link的座標系關係，請求解laser相對於camera的座標系關係。
案例2： 在 5.3 座標變換廣播 中發佈了laser相對於base_link的座標系關係且發佈了laser座標系下的障礙物的座標點數據，請求解base_link座標系下該障礙物的座標。
案例分析
在上述案例中，案例1是多座標系的場景下實現不同座標系之間的變換，案例2則是要實現同一座標點在不同座標系下的變換，雖然需求不同，但是相關算法都被封裝好了，我們只需要調用相關 API 即可。
流程簡介
兩個案例的實現流程類似，主要步驟如下：
- 編寫座標變換程序實現；
- 編輯配置文件；
- 編譯；
- 執行。
案例我們會採用 C++ 和 Python 分別實現，二者都遵循上述實現流程。
準備工作
終端下進入工作空間的src目錄，調用如下兩條命令分別創建C++功能包和Python功能包。
ros2 pkg create cpp04_tf_listener --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs --node-name demo01_tf_listener
座標 系 變換監聽_C++
實例分析
與之前實現不太一樣，要保存到buffer中。
因為之前的廣播是發一條訂閱一條。
但是在座標變換中，是多對一實現的。
多個廣播發布的消息組成一個座標樹，要從座標樹中獲取不同座標幀的變換。
把多條廣播方的消息組成座標樹，就要使用buffer，把消息全部存到緩存buffer中。
這裡為何要做異常處理呢？
因為進程間的通信開銷比較大，是有延遲的，可能程序要開始做變換了，可惜消息還沒訂閱到。
消息都沒有，就會拋異常。直到buffer裡有數據，座標也轉化成功，才不會拋異常。
實現
/*
需求：订阅 laser 到 base_link 以及 camera 到 base_link 的坐标系关系，
并生成 laser 到 camera 的坐标变换。
步骤：
1.包含头文件；
2.初始化 ROS 客户端；
3.定义节点类；
3-1.创建tf缓存对象指针；融合多个坐标系相对关系为一棵坐标树。
3-2.创建tf监听器；指定缓存对象，会将所有广播器广播的数据写入缓存。
3-3.编写定时器，循环实现转换；按照条件查找符合条件的坐标系并生成变换后的坐标帧。
4.调用 spin 函数，并传入对象指针；
5.释放资源。
*/
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2/LinearMath/Quaternion.h"
using namespace std::chrono_literals;
// 3.定义节点类；
class MinimalFrameListener : public rclcpp::Node {
public:
MinimalFrameListener():Node("minimal_frame_listener"){
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
timer_ = this->create_wall_timer(1s, std::bind(&MinimalFrameListener::on_timer,this));
}
private:
void on_timer(){
try
{
auto transformStamped = tf_buffer_->lookupTransform("camera","laser",tf2::TimePointZero);
RCLCPP_INFO(this->get_logger(),"----------------------转换结果----------------------");
RCLCPP_INFO(this->get_logger(),"frame_id:%s",transformStamped.header.frame_id.c_str());
RCLCPP_INFO(this->get_logger(),"child_frame_id:%s",transformStamped.child_frame_id.c_str());
RCLCPP_INFO(this->get_logger(),"坐标:(%.2f,%.2f,%.2f)",
transformStamped.transform.translation.x,
transformStamped.transform.translation.y,
transformStamped.transform.translation.z);
}
catch(const tf2::LookupException& e)
{
RCLCPP_INFO(this->get_logger(),"坐标变换异常：%s",e.what());
}
}
rclcpp::TimerBase::SharedPtr timer_;
std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
};
int main(int argc, char const *argv[])
{
// 2.初始化 ROS 客户端；
rclcpp::init(argc,argv);
// 4.调用 spin 函数，并传入对象指针；
rclcpp::spin(std::make_shared<MinimalFrameListener>());
// 5.释放资源。
rclcpp::shutdown();
return 0;
}
要填回調函數
實現座標系轉換的核心函數就是lookuptransform()
target_frame就是父級
source_frame就是子級
time是時間，一般都寫最新時刻的
如果buffer沒捕獲到，拋異常的函數實現
這樣就成功轉換座標了，可以打印轉換的座標。
除了用try catch處理轉換異常，還可以用buffer底下的函數轉換。
比如這個cantransform可以判斷是否可以正常轉換。
這種報錯要改為C風格字符串
啟動三個終端，終端1輸入如下命令發佈雷達（laser）相對於底盤（base_link）的靜態座標變換：
ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id camera --x -0.5 --z -0.4
終端2輸入如下命令發佈攝像頭（camera）相對於底盤（base_link）的靜態座標變換：
ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser--x 0.4 --z 0.2
終端3輸入如下命令執行座標系變換：
. install/setup.bash
ros2 run cpp04_tf_listener demo01_tf_listener
終端3將輸出 laser 相對於 camera 的座標，具體結果請參考案例1。
當只運行一個廣播，他會拋錯。
當所有廣播都跑起來時，才會正常轉換。
座標 點 變換監聽_C++
實現框架
求laser測得的座標點到base_link的距離
laser到point的座標用的是point話題，與監聽器用的話題不一樣，所以不好訂閱，要創建一個新的訂閱對象。
框架搭建
package.xml在創建功能包時，所依賴的部分功能包已經自動配置了，不過為了實現座標點變換，還需要添加依賴包tf2_geometry_msgs和
message_filters，修改後的配置內容如下：
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>geometry_msgs</depend>
<depend>tf2_geometry_msgs</depend>
<depend>message_filters</depend>
CMakeLists.txt 文件修改後的內容如下：
find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(message_filters REQUIRED)
add_executable(demo01_tf_listener src/demo01_tf_listener.cpp)
ament_target_dependencies(
demo01_tf_listener
"rclcpp"
"tf2"
"tf2_ros"
"geometry_msgs"
)
add_executable(demo02_message_filter src/demo02_message_filter.cpp)
ament_target_dependencies(
demo02_message_filter
"rclcpp"
"tf2"
"tf2_ros"
"geometry_msgs"
"tf2_geometry_msgs"
"message_filters"
)
install(TARGETS demo01_tf_listener
demo02_message_filter
DESTINATION lib/${PROJECT_NAME})
實現
/*
需求：将雷达感知到的障碍物的坐标点数据（相对于 laser 坐标系），
转换成相对于底盘坐标系（base_link）的坐标点。
步骤：
1.包含头文件；
2.初始化 ROS 客户端；
3.定义节点类；
3-1.创建tf缓存对象指针；
3-2.创建tf监听器；
3-3.创建坐标点订阅方并订阅指定话题；
3-4.创建消息过滤器过滤被处理的数据；
3-5.为消息过滤器注册转换坐标点数据的回调函数。
4.调用 spin 函数，并传入对象指针；
5.释放资源。
*/// 1.包含头文件；#include <geometry_msgs/msg/point_stamped.hpp>#include <message_filters/subscriber.h>#include <rclcpp/rclcpp.hpp>#include <tf2_ros/buffer.h>#include <tf2_ros/create_timer_ros.h>#include <tf2_ros/message_filter.h>#include <tf2_ros/transform_listener.h>// #ifdef TF2_CPP_HEADERS// #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>// #else// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>// #endif#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>using namespace std::chrono_literals;
// 3.定义节点类；class MessageFilterPointListener : public rclcpp::Node
{
public:
MessageFilterPointListener(): Node("message_filter_point_listener")
{
target_frame_ = "base_link";
typedef std::chrono::duration<int> seconds_type;
seconds_type buffer_timeout(1);
// 3-1.创建tf缓存对象指针；
tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
this->get_node_base_interface(),
this->get_node_timers_interface());
tf2_buffer_->setCreateTimerInterface(timer_interface);
// 3-2.创建tf监听器；
tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
// 3-3.创建坐标点订阅方并订阅指定话题；
point_sub_.subscribe(this, "point");
// 3-4.创建消息过滤器过滤被处理的数据；
tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),
this->get_node_clock_interface(), buffer_timeout);
// 3-5.为消息过滤器注册转换坐标点数据的回调函数。
tf2_filter_->registerCallback(&MessageFilterPointListener::msgCallback, this);
}
private:
void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr){
geometry_msgs::msg::PointStamped point_out;
try {
tf2_buffer_->transform(*point_ptr, point_out, target_frame_);
RCLCPP_INFO(
this->get_logger(), "坐标点相对于base_link的坐标:(%.2f,%.2f,%.2f)",
point_out.point.x,
point_out.point.y,
point_out.point.z);
} catch (tf2::TransformException & ex) {
RCLCPP_WARN(
// Print exception which was caughtthis->get_logger(), "Failure %s\n", ex.what());
}
}
std::string target_frame_;
std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;
std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;
};
int main(int argc, char * argv[]){
// 2.初始化 ROS 客户端；
rclcpp::init(argc, argv);
// 4.调用 spin 函数，并传入对象指针；
rclcpp::spin(std::make_shared<MessageFilterPointListener>());
// 5.释放资源。
rclcpp::shutdown();
return 0;
}
把上面此參數設置一下
進行數據解析
用第一個重載。
轉換過程會拋異常，可以不管他。轉化失敗會在終端上自動拋異常。
在當前工作空間下，啟動三個終端，終端1輸入如下命令發佈雷達（laser）相對於底盤（base_link）的靜態座標變換：
. install/setup.bash
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0 0.2 0 0 0 base_link laser
終端2輸入如下命令發佈障礙物相對於雷達（laser）的座標點：
. install/setup.bash
ros2 run cpp03_tf_broadcaster demo03_point_publisher
終端3輸入如下命令執行座標點變換：
. install/setup.bash
ros2 run cpp04_tf_listener demo02_message_filter
終端3將輸出座標點相對於 base_link 的座標，具體結果請參考案例2。
按順序依次發佈
座標變換工具
在 ROS2 的 TF 框架中除了封裝了座標廣播與訂閱功能外，還提供了一些工具，可以幫助我們提高開發、調試效率。本節主要介紹這些工具的使用，這些工具主要涉及到兩個功能包：tf2_ros和tf2_tools。
tf2_ros包中提供的常用節點如下：
- static_transform_publisher：該節點用於廣播靜態座標變換；（用過很多次了）
- tf2_monitor：該節點用於打印所有或特定座標系的發佈頻率與網絡延遲；
- tf2_echo：該節點用於打印特定座標系的平移旋轉關係。
tf2_tools包中提供的節點如下：
- view_frames：該節點可以生成顯示座標系關係的 pdf 文件，該文件包含樹形結構的座標系圖譜。
上述諸多工具中，功能包tf2_ros中的static_transform_publisher節點在 靜態廣播器（命令） 一節中已有詳細說明，本節不再介紹。
準備工作：
為了更好的演示工具的使用，請先啟動若干座標系廣播節點，比如：可以按照 靜態廣播器（命令） 和 動態廣播器（C++） 廣播一些座標系消息。
第一個終端：
ros2 run tf2_ros static_transform_publisher --x 0.4 --y 0 --z 0.2 --yaw 0.5 --roll 0 --pitch 0 --frame-id base_link --child-frame-id laser
第二個終端：
ros2 run tf2_ros static_transform_publisher --x -0.5 --y 0 --z 0.4 --yaw 0 --roll 0 --pitch 0 --frame-id base_link --child-frame-id camera
第三個終端：
ros2 run turtlesim turtlesim_node
第四個終端：
ros2 run turtlesim turtle_teleop_key
第五個終端：
source install/setup.bash
ros2 run cpp03_tf_broadcaster demo02_dynamic_tf_broadcaster
以上是準備工作
1.tf2_monitor
1.1打印所有座標系的發佈頻率與網絡延遲
終端執行命令：
ros2 run tf2_ros tf2_monitor
運行結果：
該命令有10s的阻塞時間，因為要在一個區間內測頻率。
靜態的沒有變化，而動態的發佈頻率是有一些變化的。
該函數每隔10s會再發布一次。
1.2打印指定座標系的發佈頻率與網絡延遲
終端執行命令：
ros2 run tf2_ros tf2_monitor camera laser
運行結果：
剛開始在緩存裡拿不到數據拋錯很正常。
2.tf2_echo
打印兩個座標系的平移旋轉關係。
終端執行命令：
ros2 run tf2_ros tf2_echo world turtle1
運行結果：
會出平移量和旋轉量
會以好幾種方式表現，如平移距離，四元數，弧度歐拉角，角度歐拉角，矩陣等。
當龜男🐢🚹動彈的時候，數值也會發生改變。
3.view_frames（常用）
以圖形化的方式顯示座標系關係。
終端執行命令：
ros2 run tf2_tools view_frames
運行結果：將會在當前目錄下生成 frames_xxxx.gv 與 frames_xxxx.pdf 文件，其中 xxxx 為時間戳。打開 pdf 文件顯示如下內容：
此節點會監聽我們5秒鐘，並生成對應的文件。
上圖中有兩棵座標樹，但是實際上的項目中，咱們只能設計一棵樹，而不能設計兩個座標樹。