#include #include "modulecomm.h" #include "xmlparam.h" #include "odom.pb.h" #include "gpsimu.pb.h" #include #include #include "math/gnss_coordinate_convert.h" #include struct Quaternion { double w, x, y, z; }; struct EulerAngles { double roll, pitch, yaw; }; EulerAngles ToEulerAngles(Quaternion q) { EulerAngles angles; // roll (x-axis rotation) double sinr_cosp = 2 * (q.w * q.x + q.y * q.z); double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y); angles.roll = std::atan2(sinr_cosp, cosr_cosp); // pitch (y-axis rotation) double sinp = 2 * (q.w * q.y - q.z * q.x); if (std::abs(sinp) >= 1) angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range else angles.pitch = std::asin(sinp); // yaw (z-axis rotation) double siny_cosp = 2 * (q.w * q.z + q.x * q.y); double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z); angles.yaw = std::atan2(siny_cosp, cosy_cosp); return angles; } Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X) { // Abbreviations for the various angular functions double cy = cos(yaw * 0.5); double sy = sin(yaw * 0.5); double cp = cos(pitch * 0.5); double sp = sin(pitch * 0.5); double cr = cos(roll * 0.5); double sr = sin(roll * 0.5); Quaternion q; q.w = cy * cp * cr + sy * sp * sr; q.x = cy * cp * sr - sy * sp * cr; q.y = sy * cp * sr + cy * sp * cr; q.z = sy * cp * cr - cy * sp * sr; return q; } //原文链接:https://blog.csdn.net/xiaoma_bk/article/details/79082629 static double glon0,glat0,gheight0,ghead0; static void * gpa; void ListenODOM(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { (void)&index; (void)dt; (void)strmemname; iv::ros::odom xodom; if(xodom.ParseFromArray(strdata,nSize) == false) { std::cout<<" ListenODOM Fail. "<360)heading = heading - 360; xgpsimu.set_lat(lat); xgpsimu.set_lon(lon); xgpsimu.set_height(xodom.pose_position().z() + gheight0); xgpsimu.set_heading(heading); xgpsimu.set_pitch(xAng.pitch); xgpsimu.set_roll(xAng.roll); double vx,vy; vx = fabs(xodom.twist_linear().x() *cos(xAng.yaw) + xodom.twist_linear().y() * sin(xAng.yaw)); vy = fabs(xodom.twist_linear().x() *sin(xAng.yaw) + xodom.twist_linear().y() * cos(xAng.yaw)); double ve = vx * cos(hdg_o) - vy * sin(hdg_o); double vn = vx * sin(hdg_o) + vy * cos(hdg_o); double vd = xodom.twist_linear().z() * (-1); xgpsimu.set_ve(ve); xgpsimu.set_vn(vn); xgpsimu.set_vd(vd); xgpsimu.set_ins_state(4); xgpsimu.set_rtk_state(6); xgpsimu.set_satnum1(1); xgpsimu.set_satnum2(1); int nbytesize = xgpsimu.ByteSize(); std::shared_ptr str_ptr = std::shared_ptr(new char[nbytesize]); if(xgpsimu.SerializeToArray(str_ptr.get(),nbytesize)) { iv::modulecomm::ModuleSendMsg(gpa,str_ptr.get(),nbytesize); } else { std::cout<<" ListenODOM Serizlize Error."<