坐标变换的含义
假设有一个小车，小车上有一个雷达，右边是一堵墙，已知小车的坐标原点 base_link 与雷达坐标原点 lase_laser 横竖距离 10cm , 既雷达相对小车的坐标为 (10, 0, 10),
雷达探测到墙体的坐标为 （60, 0, 0） ，求墙体相对小车的坐标？
上面这个问题实际就是坐标变换，针对这个问题，我们可以手算出来为 (70, 0, 10) , 但是在实际的场景下需要一个工具能够快速算出对应的坐标。
ROS2 提供了 TF2 这个工具包来实现这个功能。
ROS2 TF2 命令行工具
发布 base_link 到 base_laser 的坐标
1
| ros2 run tf2_ros static_transform_publisher --x 10 --y 0 --z 10 --roll 0 --pitch 0 --yaw 0 --frame-id base_link --child-frame-id base_laser
|
roll pitch yaw 为旋转角度，既 tf2 支持旋转的变换
发布墙体 wall_point 相对 base_laser 的坐标
1
| ros2 run tf2_ros static_transform_publisher --x 60 --y 0 --z 0 --roll 0 --pitch 0 --yaw 0 --frame-id base_laser --child-frame-id wall_point
|
然后就可以查询 wall_point 相对 base_laser 的坐标
1
| ros2 run tf2_ros tf2_echo base_link wall_point
|
查询到的结果和我们手算的一致：
1
| Translation: [70.000, 0.000, 10.000]
|
工具的原理就是向 /tf_static 这个 topic 发布消息，可以查看详细信息 ros2 topic info /tf_static
1
2
3
| Type: tf2_msgs/msg/TFMessage
Publisher count: 2
Subscription count: 0
|
ros2 interface show tf2_msgs/msg/TFMessage 查看 topic 信息
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
| geometry_msgs/TransformStamped[] transforms
#
#
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
|
/tf_static 用来发布固定的坐标关系，既静态发布，比如雷达固定在小车上，相对小车的坐标不会改变/tf 用来发布动态坐标关系，既动态发布，比如小车雷达一直在运动，相对墙体的坐标一直在变化
C++ 应用坐标变化
创建一个功能包
1
| ros2 pkg create cpp_tf_demo --destination-directory=src --build-type=ament_cmake --license Apache-2.0 --dependencies rclcpp tf2_ros geometry_msgs tf2_geometry_msgs
|
发布静态 TF
通过静态 TF 发布 base_link 和 base_laser 之间的坐标关系
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
| #include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/executors.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/static_transform_broadcaster.hpp"
class StaticTf : public rclcpp::Node {
public:
StaticTf() : Node("StaticTf") {
broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
// publish
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = get_clock()->now(); // 时间戳
transform.header.frame_id = "base_link"; // 父坐标系名称
transform.child_frame_id = "base_laser"; // 子坐标系名称
// 坐标 (10, 0, 10)
transform.transform.translation.x = 10;
transform.transform.translation.y = 0;
transform.transform.translation.z = 10;
// 旋转角度为 0
tf2::Quaternion q;
q.setRPY(0, 0, 0);
transform.transform.rotation = tf2::toMsg(q);
// 发布消息
broadcaster_->sendTransform(transform);
}
private:
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
};
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<StaticTf>());
rclcpp::shutdown();
return 0;
}
|
发布动态 TF
通过动态 TF 发布 base_laser 和 wall_point 之间的坐标关系 ， base_laser 每秒移动 10cm
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
| #include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/executors.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_broadcaster.hpp"
using namespace std::chrono_literals;
class DynamicTf : public rclcpp::Node {
public:
DynamicTf() : Node("DynamicTf"), count_(0) {
broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
timer_ = create_wall_timer(1s, [this]{
// publish
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = get_clock()->now(); // 时间戳
transform.header.frame_id = "base_laser"; // 父坐标系名称
transform.child_frame_id = "wall_point"; // 子坐标系名称
transform.transform.translation.x = 60 + count_ * 10; // 每秒增加 10 cn
transform.transform.translation.y = 0;
transform.transform.translation.z = 0;
// 旋转角度为 0
tf2::Quaternion q;
q.setRPY(0, 0, 0);
transform.transform.rotation = tf2::toMsg(q);
// 发布消息
broadcaster_->sendTransform(transform);
count_++;
});
}
private:
int count_;
std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<DynamicTf>());
rclcpp::shutdown();
return 0;
}
|
定时查询指定坐标
现在可以查询 wall_point 相对 base_link 的坐标
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
| #include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/executors.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.hpp"
using namespace std::chrono_literals;
class ListenTf : public rclcpp::Node {
public:
ListenTf() : Node("ListenTf") {
buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock());
listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
timer_ = create_wall_timer(1s, std::bind(&ListenTf::listen, this));
}
void listen() {
try {
// 查询坐标关系
const auto transform = buffer_->lookupTransform("base_link", "wall_point", tf2::TimePointZero);
auto translation = transform.transform.translation;
RCLCPP_INFO(this->get_logger(), "transform: (%f, %f, %f)" , translation.x, translation.y, translation.z);
} catch (const tf2::TransformException& e) {
RCLCPP_WARN(this->get_logger(), "Could not transform: %s" , e.what());
}
}
private:
std::shared_ptr<tf2_ros::TransformListener> listener_;
rclcpp::TimerBase::SharedPtr timer_;
std::unique_ptr<tf2_ros::Buffer> buffer_;
};
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<ListenTf>());
rclcpp::shutdown();
return 0;
}
|
通过 launch 机制同时启动三个 Node ，Listen 节点就会输出转换的坐标
1
2
3
4
| [cpp_listen_tf-3] [INFO] [1767545900.414608372] [ListenTf]: transform: (80.000000, 0.000000, 10.000000)
[cpp_listen_tf-3] [INFO] [1767545901.464255733] [ListenTf]: transform: (90.000000, 0.000000, 10.000000)
[cpp_listen_tf-3] [INFO] [1767545902.464750606] [ListenTf]: transform: (100.000000, 0.000000, 10.000000)
[cpp_listen_tf-3] [INFO] [1767545903.429858553] [ListenTf]: transform: (110.000000, 0.000000, 10.000000)
|
