ROS TF 坐标系转换示例：订阅坐标系变换并转换坐标点

#include <ros/ros.h> #include <tf2_ros/transform_listener.h> #include <tf2/LinearMath/Quaternion.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include <geometry_msgs/TransformStamped.h> #include <geometry_msgs/PointStamped.h>

int main(int argc, char *argv[]) { setlocale(LC_ALL, ""); // 2. 初始化 ros 节点 ros::init(argc, argv, "sub_frames"); // 3. 创建 ros 句柄 ros::NodeHandle nh; // 4. 创建 TF 订阅对象 tf2_ros::Buffer buffer; tf2_ros::TransformListener listener(buffer); // 5. 解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标 ros::Rate r(1); while (ros::ok()) { try { // 解析 son1 中的点相对于 son2 的坐标 geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2", "son1", ros::Time(0)); ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s", tfs.header.frame_id.c_str()); ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s", tfs.child_frame_id.c_str()); ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f", tfs.transform.translation.x, tfs.transform.translation.y, tfs.transform.translation.z); // 坐标点解析 geometry_msgs::PointStamped ps; ps.header.frame_id = "son1"; ps.header.stamp = ros::Time::now(); ps.point.x = 1.0; ps.point.y = 2.0; ps.point.z = 3.0; geometry_msgs::PointStamped psAtSon2; psAtSon2 = buffer.transform(ps, "son2"); ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f", psAtSon2.point.x, psAtSon2.point.y, psAtSon2.point.z); } catch (const std::exception& e) { // std::cerr << e.what() << '\n'; ROS_INFO("异常信息:%s", e.what()); } r.sleep(); // 6. spin ros::spinOnce(); } return 0; }