#include"ros/ros.h"
#include"std_msgs/String.h"
#include<pcl/common/transforms.h>
#include"nav_msgs/Odometry.h"
#include <tf/tf.h>
#include <tf_conversions/tf_eigen.h>
#include<Eigen/Eigen>
// 可用于geometry_msgs与Eigen相关数据类型之间的转换。
#include <eigen_conversions/eigen_msg.h>
#include<pcl/visualization/pcl_visualizer.h>
#include<pcl/point_cloud.h>
// 包含pcd的输入输出头文件——pcd文件IO操作定义的头文件
#include <pcl/io/pcd_io.h>
// 包含点类型、结构的数据定义——包含若干PointT数据结构定义的头文件
#include <pcl/point_types.h>
using namespace std;
// ConstPtr常量指针可以修改指针的方向,不可以修改指针的数值
void doMsg(const nav_msgs::Odometry::ConstPtr &gps_msg)
{
// 通过msg获取并操作订阅到的数据
// ROS_INFO("翠花订阅的数据:%s",msg->data.c_str());
Eigen::Affine3d gps_aff;
cout<<"gps_aff="<<gps_aff.matrix()<<endl;
// 将获取的gps坐标转换为4维的矩阵
tf::poseMsgToEigen(gps_msg->pose.pose,gps_aff);
cout<<gps_aff.matrix()<<endl;
// 强制类型转换
Eigen::Matrix4d gps_matrix=gps_aff.matrix().cast<double>();;
cout<<"gps_matrix="<<endl;
cout<<gps_matrix.matrix()<<endl;
// 定义四元素,用四元素表示地理到载体坐标系
Eigen::Quaterniond q;
q.x()=gps_msg->pose.pose.orientation.x;
q.y()=gps_msg->pose.pose.orientation.y;
q.z()=gps_msg->pose.pose.orientation.z;
q.w()=gps_msg->pose.pose.orientation.w;
Eigen::Matrix3d A;
A(0,0)=2*q.w()*q.w()+2*pow(q.x(),2)-1;
A(0,1)=2*q.x()*q.y()+2*q.w()*q.z();
A(0,2)=2*q.x()*q.z()-2*q.w()*q.y();
A(1,0)=2*q.x()*q.y()-2*q.w()*q.z();
A(1,1)=2*q.w()*q.w()+2*q.y()*q.y()-1;
A(1,2)=2*q.y()*q.z()+2*q.w()*q.x();
A(2,0)=2*q.x()*q.z()+2*q.w()*q.y();
A(2,1)=2*q.y()*q.z()-2*q.w()*q.x();
A(2,2)=2*q.w()*q.w()+2*q.z()*q.z()-1;
cout<<"A="<<endl;
cout<<A<<endl;
// 那么载体i坐标系转地理坐标系的方向余弦矩阵就为C,为A的转置
Eigen::Matrix3d C;
C=A.transpose();
cout<<"C="<<endl;
cout<<C<<endl;
}
int main(int argc, char *argv[])
{
// 解决中文乱码问题
setlocale(LC_ALL,"");
// 初始化ros节点
ros::init(argc,argv,"GuiHua");
// 创建节点句柄
ros::NodeHandle nh;
// 创建订阅者对象
ros::Subscriber sub=nh.subscribe("/sensing/gnss/gps/odom",10,doMsg);
ros::spin();
return 0;
}
实验结果如下:
程序代码解析:
载体坐标系与地理坐标系间的坐标转换关系
[XgYgZg]=C[XYZ]
(1)获取gps位置信息将其转换为eigen
tf::poseMsgToEigen(gps_msg->pose.pose,gps_aff);
(2)用四元素来表示C
(3)程序运行后发现tf::poseMsgToEigen与C是等价的
(4)tf::poseMsgToEigen获取的pose与四元素解析
其获取的四元素为载体坐标系下的旋转角度,需要用C[Wx]将其转换到地理坐标系下
而其获取的pose为地理坐标下的坐标值