【ROS】 tf2 Pose 的坐標變換

 

求 將 geometry_msgs/Pose 的數據從一個坐標系轉換到另一個坐標系 后的position 和 orientation

 四元數表示剛體姿態？？？

tf2具體使用

方法1 利用 tf2::transform  和 geometry_msgs/PoseStamped   (  tf2_geometry_msgs    tf2 )

#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;
}

 

 

方法2 利用 tf2::transform  和 geometry_msgs/PointStamped  以及 四元數相乘 （ tf2教程      tf2四元數使用   四元數與歐拉角轉換 ）

#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; }

 

測試期間遇到的錯誤(尚未發現具體問題，只是重寫了代碼，去掉了 lookuptransform的調用)

lookuptransform 的時間戳問題  

Could not find a connection between frames