【ROS】 tf2 Pose 的坐标变换

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"

#include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"  // 调用transform 必须包含该头文件   否则编译不通过

/*
    订阅发布的两个坐标系的关系
    并将一个pose在子坐标系中的坐标值   转换到 父坐标系
*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tf_sub");
    ros::NodeHandle nh;

    tf2_ros::Buffer buffer; // 创建 TF 订阅节点
    tf2_ros::TransformListener listener(buffer);

    ros::Rate rate(1);
    ros::Duration(3).sleep();  // 注意别写错   不加的话 转换的时候会报错
    while (ros::ok())
    {
        geometry_msgs::PoseStamped pose_before, pose_transformed;
        pose_before.header.frame_id = "base_link";
        pose_before.header.stamp = ros::Time(0.0);
        pose_before.pose.position.x = 2;
        pose_before.pose.position.y = 2;
        pose_before.pose.position.z = 2;
        tf2::Quaternion qtn;
        qtn.setRPY(0,0,-1.57);  // 单位是弧度
        pose_before.pose.orientation.x = qtn.getX();
        pose_before.pose.orientation.y = qtn.getY();
        pose_before.pose.orientation.z = qtn.getZ();
        pose_before.pose.orientation.w = qtn.getW();
        try
        {
            pose_transformed = buffer.transform(pose_before,"map");  // map 是要计算的坐标系    pose_transformed 在 map 里的坐标 

            tf2::Quaternion q_pose;   // 四元数转换s到 欧拉角 方便打印判断
            tf2::convert(pose_transformed.pose.orientation ,q_pose);
            double roll, pitch, yaw;//定义存储r\p\y的容器
            tf2::Matrix3x3 m(q_pose);
            m.getRPY(roll, pitch, yaw);//进行转换

            ROS_INFO("转换后的数据: (%.2f,%.2f,%.2f)； orientation(%.2f,%.2f,%.2f), 参考的坐标系是: %s",pose_transformed.pose.position.x,pose_transformed.pose.position.y, 
            pose_transformed.pose.position.z, 
            roll, pitch, yaw,  pose_transformed.header.frame_id.c_str());  // 不加%s会报错
        }
        catch(const std::exception& e)
        {
            //std::cerr << e.what() << '\n';
            ROS_INFO("程序异常");
        }
        
        rate.sleep();
    }
    
    return 0;
}

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"

#include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/TransformStamped.h"

#include "tf2_geometry_msgs/tf2_geometry_msgs.h"  // 调用transform 必须包含该头文件   否则编译不通过


/*
    订阅发布的两个坐标系的关系
    并将一个pose在子坐标系中的坐标值   转换到 父坐标系

*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tf_sub");
    ros::NodeHandle nh;

    tf2_ros::Buffer buffer; // 创建 TF 订阅节点
    tf2_ros::TransformListener listener(buffer);

    ros::Rate rate(1);
    ros::Duration(3).sleep();  // 注意别写错   不加的话 转换的时候会报错
    while (ros::ok())
    {
        geometry_msgs::Pose pose_before, pose_transformed;
        pose_before.position.x = 2;
        pose_before.position.y = 2;
        pose_before.position.z = 2;
        pose_before.orientation.x = 0;
        pose_before.orientation.y = 0;
        pose_before.orientation.z = 0;
        pose_before.orientation.w = 1;

        geometry_msgs::PointStamped point_transformed, point_before;

        point_before.header.frame_id = "base_link";
        point_before.header.stamp = ros::Time::now();

        point_before.point.x = pose_before.position.x;
        point_before.point.y = pose_before.position.y;
        point_before.point.z = pose_before.position.z;
        tf2::Quaternion q_pose, q_rota,q_outp; 
        try
        {
            
            point_transformed = buffer.transform(point_before,"map");  // map 是要计算的坐标系    point_transformed 在 map 里的坐标 
            ROS_INFO("转换后的数据: (%.2f,%.2f,%.2f), 参考的坐标系是: %s",point_transformed.point.x,point_transformed.point.y,point_transformed.point.z,point_transformed.header.frame_id.c_str());  // 不加%s会报错

            geometry_msgs::TransformStamped tf_base2map;
            tf_base2map = buffer.lookupTransform("map","base_link",ros::Time(0));

            tf2::convert(tf_base2map.transform.rotation ,q_rota);
            tf2::convert(pose_before.orientation ,q_pose);//通过tf2::convert()将msg格式转换成tf格式

            q_outp = q_rota*q_pose;  //旋转后的姿态 = 旋转的四元数* 原姿态

 q_outp.normalize(); pose_transformed.orientation = tf2::toMsg(q_outp); // ？？？ orientation 怎么变换  pose_transformed.position.x = point_transformed.point.x; pose_transformed.position.y = point_transformed.point.y; pose_transformed.position.z = point_transformed.point.z; //tf2::Quaternion q_pose; // 四元数转换 欧拉角  tf2::convert(pose_transformed.orientation ,q_pose); double roll, pitch, yaw;//定义存储r\p\y的容器  tf2::Matrix3x3 m(q_pose); m.getRPY(roll, pitch, yaw);//进行转换  ROS_INFO("转换后的数据: orientation(%.2f,%.2f,%.2f)", roll, pitch, yaw); // 不加%s会报错  } catch(const std::exception& e) { //std::cerr << e.what() << '\n'; ROS_INFO("程序异常"); } rate.sleep(); } return 0; }