瞎忙好几天，草草做个总结，贴上混乱代码一堆 涉及内容如下
dmp iic mpu6050的通讯
获取加速度 角速度
二者结合求出角姿势 四元数，发送到计算机。
串口协议编写解析
ros发布位姿里程数据
试验了卡尔曼滤波 ，
加速度积分成位移（漂移问题）
加速度转到频域下积分成位移（高频）参考文章
fft变换卡尔曼滤波等算法
结论：
mpu6050做角姿还不错，求位置很难。
是否是精度问题？噪声问题？低精度信号淹没在大噪声中？
算法问题？没有合适的算法求出纯加速度，进行恰当的滤波？
有待深入学习。
arduino 使用官方库的示例mpu_dmp.ino，使用自定格式串口通讯，部分代码如下；
#ifdef OUTPUT_READABLE_WORLDACCEL// display initial world-frame acceleration, adjusted to remove gravity// and rotated based on known orientation from quaternionmpu.dmpGetQuaternion(&q, fifoBuffer);mpu.dmpGetAccel(&aa, fifoBuffer);mpu.dmpGetGravity(&gravity, &q);mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);byte* pw=(byte*)&(q.w);byte* px=(byte*)&(q.x);byte* py=(byte*)&(q.y);byte* pz=(byte*)&(q.z);byte* pwx=(byte*)&(aa.x);byte* pwy=(byte*)&(aa.y);byte* pwz=(byte*)&(aa.z);byte data[26]={'$',0x1a,*pw,*(pw+1),*(pw+2),*(pw+3),*px,*(px+1),*(px+2),*(px+3),*py,*(py+1),*(py+2),*(py+3),*pz,*(pz+1),*(pz+2),*(pz+3),*pwx,*(pwx+1),*pwy,*(pwy+1),*pwz,*(pwz+1),'\r', '\n' };Serial.write(data, 26);/* Serial.print("aworld\t");Serial.print(aaWorld.x);Serial.print("\t");Serial.print(aaWorld.y);Serial.print("\t");Serial.println(aaWorld.z);*/#endif
ros rviz可视化：
冗长的代码：
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
//#include "gy85/imu.h"
#include <serial/serial.h>
//ROS已经内置了的串口包#include <boost/asio.hpp> //包含boost库函数
#include "math.h"
#include "helper_3dmath.h"
#include "polyfit.hpp"
#define DOUBLE_PI 6.283185307179586476925286766559
#define DATA_LENGTH 16
// 一维滤波器信息结构体
typedef struct{double a; //double b; // Kalamn增益} complex;complex dataX[DATA_LENGTH];
complex dataY[DATA_LENGTH];
complex dataZ[DATA_LENGTH];
complex odataX[DATA_LENGTH];
complex odataY[DATA_LENGTH];
complex odataZ[DATA_LENGTH];
complex r1dataX[DATA_LENGTH];
complex r1dataY[DATA_LENGTH];
complex r1dataZ[DATA_LENGTH];
complex r2dataX[DATA_LENGTH];
complex r2dataY[DATA_LENGTH];
complex r2dataZ[DATA_LENGTH];
double dataXA[DATA_LENGTH];
double dataYA[DATA_LENGTH];
double dataZA[DATA_LENGTH];
double dataXB[DATA_LENGTH];
double dataYB[DATA_LENGTH];
double dataZB[DATA_LENGTH];
void resfft1(){unsigned int n=0;for(n=0;n<DATA_LENGTH;n++){r1dataX[n].a= dataX[n].a;r1dataX[n].b= dataX[n].b;r1dataY[n].a= dataY[n].a;r1dataY[n].b= dataY[n].b;r1dataZ[n].a= dataZ[n].a;r1dataZ[n].b= dataZ[n].b;}
}
void prefft(){unsigned int n=0;for(n=0;n<DATA_LENGTH;n++){dataX[n].a= odataX[n].a;dataX[n].b= odataX[n].b;dataY[n].a= odataY[n].a;dataY[n].b= odataY[n].b;dataZ[n].a= odataZ[n].a;dataZ[n].b= odataZ[n].b;}
}
void c2d(){unsigned int n=0;for(n=0;n<DATA_LENGTH;n++){dataXA[n]= dataX[n].a;dataXB[n]= dataX[n].b;dataYA[n]= dataY[n].a;dataYB[n]= dataY[n].b;dataZA[n]= dataZ[n].a;dataZB[n]= dataZ[n].b;}}
void d2c(){unsigned int n=0;for(n=0;n<DATA_LENGTH;n++){dataX[n].a=dataXA[n];dataX[n].b=dataXB[n];dataY[n].a=dataYA[n];dataY[n].b=dataYB[n];dataZ[n].a=dataZA[n];dataZ[n].b=dataZB[n];}}unsigned char datan=0;const int N = 1024;
const double PI = 3.14159265358979;inline void swap (double &a, double &b)
{double t;t = a;a = b;b = t;
}void bitrp (double xreal [], double ximag [], int n)
{// 位反转置换 Bit-reversal Permutationint i, j, a, b, p;for (i = 1, p = 0; i < n; i *= 2){p ++;}for (i = 0; i < n; i ++){a = i;b = 0;for (j = 0; j < p; j ++){b = (b << 1) + (a & 1); // b = b * 2 + a % 2;a >>= 1; // a = a / 2;}if ( b > i){swap (xreal [i], xreal [b]);swap (ximag [i], ximag [b]);}}
}void FFT(double xreal [], double ximag [], int n)
{// 快速傅立叶变换，将复数 x 变换后仍保存在 x 中，xreal, ximag 分别是 x 的实部和虚部double wreal [N / 2], wimag [N / 2], treal, timag, ureal, uimag, arg;int m, k, j, t, index1, index2;bitrp (xreal, ximag, n);// 计算 1 的前 n / 2 个 n 次方根的共轭复数 W'j = wreal [j] + i * wimag [j] , j = 0, 1, ... , n / 2 - 1arg = - 2 * PI / n;treal = cos (arg);timag = sin (arg);wreal [0] = 1.0;wimag [0] = 0.0;for (j = 1; j < n / 2; j ++){wreal [j] = wreal [j - 1] * treal - wimag [j - 1] * timag;wimag [j] = wreal [j - 1] * timag + wimag [j - 1] * treal;}for (m = 2; m <= n; m *= 2){for (k = 0; k < n; k += m){for (j = 0; j < m / 2; j ++){index1 = k + j;index2 = index1 + m / 2;t = n * j / m; // 旋转因子 w 的实部在 wreal [] 中的下标为 ttreal = wreal [t] * xreal [index2] - wimag [t] * ximag [index2];timag = wreal [t] * ximag [index2] + wimag [t] * xreal [index2];ureal = xreal [index1];uimag = ximag [index1];xreal [index1] = ureal + treal;ximag [index1] = uimag + timag;xreal [index2] = ureal - treal;ximag [index2] = uimag - timag;}}}
}void IFFT (double xreal [], double ximag [], int n)
{// 快速傅立叶逆变换double wreal [N / 2], wimag [N / 2], treal, timag, ureal, uimag, arg;int m, k, j, t, index1, index2;bitrp (xreal, ximag, n);// 计算 1 的前 n / 2 个 n 次方根 Wj = wreal [j] + i * wimag [j] , j = 0, 1, ... , n / 2 - 1arg = 2 * PI / n;treal = cos (arg);timag = sin (arg);wreal [0] = 1.0;wimag [0] = 0.0;for (j = 1; j < n / 2; j ++){wreal [j] = wreal [j - 1] * treal - wimag [j - 1] * timag;wimag [j] = wreal [j - 1] * timag + wimag [j - 1] * treal;}for (m = 2; m <= n; m *= 2){for (k = 0; k < n; k += m){for (j = 0; j < m / 2; j ++){index1 = k + j;index2 = index1 + m / 2;t = n * j / m; // 旋转因子 w 的实部在 wreal [] 中的下标为 ttreal = wreal [t] * xreal [index2] - wimag [t] * ximag [index2];timag = wreal [t] * ximag [index2] + wimag [t] * xreal [index2];ureal = xreal [index1];uimag = ximag [index1];xreal [index1] = ureal + treal;ximag [index1] = uimag + timag;xreal [index2] = ureal - treal;ximag [index2] = uimag - timag;}}}for (j=0; j < n; j ++){xreal [j] /= n;ximag [j] /= n;}
}
void swap(complex *v1, complex *v2)
{complex tmp = *v1;*v1 = *v2;*v2 = tmp;
}void fftshift(complex *data, int count)
{int k = 0;int c = (int) floor((float)count/2);// For odd and for even numbers of element use different algorithmif (count % 2 == 0){for (k = 0; k < c; k++)swap(&data[k], &data[k+c]);}else{complex tmp = data[0];for (k = 0; k < c; k++){data[k] = data[c + k + 1];data[c + k + 1] = data[k + 1];}data[c] = tmp;}
}void ifftshift(complex *data, int count)
{int k = 0;int c = (int) floor((float)count/2);if (count % 2 == 0){for (k = 0; k < c; k++)swap(&data[k], &data[k+c]);}else{complex tmp = data[count - 1];for (k = c-1; k >= 0; k--){data[c + k + 1] = data[k];data[k] = data[c + k];}data[c] = tmp;}
}
void detrend(double array[], int n)
{double x, y, a, b;double sy = 0.0,sxy = 0.0,sxx = 0.0;int i;for (i=0, x=(-(double)n/2.0+0.5); i<n; i++, x+=1.0){y = array[i];sy += y;sxy += x * y;sxx += x * x;}b = sxy / sxx;a = sy / n;for (i=0, x=(-(double)n/2.0+0.5); i<n; i++, x+=1.0)array[i] -= (a+b*x);
}
void iomega_expCount(complex dataX[],complex iomega_array[],double iomega_exp){for(int n=0;n<DATA_LENGTH;n++){double a,b,c,d;a=dataX[n].a;b=dataX[n].b;c=iomega_array[n].a;d=iomega_array[n].b;if(iomega_array[n].a!=0||iomega_array[n].b!=0){//A(j) = A(j) * (iomega_array(j) ^ iomega_exp);//^double r =sqrt(pow(c, 2) + pow(d, 2));double sita =atan(d/c);double R = pow(r, iomega_exp)*cos(iomega_exp*sita );double I = pow(r, iomega_exp)*sin(iomega_exp*sita );//return Complex(R, I);c=R;d=I;//*//(ac－bd)+(bc+ad)idataX[n].a=(a*c-b*d);dataX[n].b=(b*c+a*d);}else{dataX[n].a=0;dataX[n].b=0;}}
}
//void iomega(datain, dt, datain_type, dataout_type)
void iomega( double dt, int datain_type,int dataout_type){double N =(double) DATA_LENGTH; //2^nextpow2(max(size(datain)));double df = 1/(N*dt);double Nyq = 1/(2*dt);complex iomega_array [DATA_LENGTH];for(int c=0;c<(int)N;c++){//iomega_array[c].a=0;iomega_array[c].b=2.0*DOUBLE_PI*(-Nyq+(double)c*df);}//iomega_array = 1i*2*pi*(-Nyq : df : Nyq-df);int iomega_exp = dataout_type - datain_type;//iomega_exp = dataout_type - datain_type;prefft();c2d();FFT(dataXA,dataXB,DATA_LENGTH);FFT(dataYA,dataYB,DATA_LENGTH);FFT(dataZA,dataZB,DATA_LENGTH);d2c();fftshift(dataX,DATA_LENGTH);fftshift(dataY,DATA_LENGTH);fftshift(dataZ,DATA_LENGTH);iomega_expCount(dataX,iomega_array,iomega_exp);iomega_expCount(dataY,iomega_array,iomega_exp);iomega_expCount(dataZ,iomega_array,iomega_exp);//ifftshift(dataX,DATA_LENGTH);ifftshift(dataY,DATA_LENGTH);ifftshift(dataZ,DATA_LENGTH);c2d();IFFT(dataXA,dataXB,DATA_LENGTH);IFFT(dataYA,dataYB,DATA_LENGTH);IFFT(dataZA,dataZB,DATA_LENGTH);d2c();}
std::vector<double> dxa(DATA_LENGTH);
std::vector<double> dya(DATA_LENGTH);
std::vector<double> dza(DATA_LENGTH);
std::vector<double> dxb(DATA_LENGTH);
std::vector<double> dyb(DATA_LENGTH);
std::vector<double> dzb(DATA_LENGTH);
void res12v(){unsigned int n=0;for(n=0;n<DATA_LENGTH;n++){dxa[n]=r1dataX[n].a;dya[n]=r1dataY[n].a;dza[n]=r1dataZ[n].a;dxb[n]=r1dataX[n].b;dyb[n]=r1dataY[n].b;dzb[n]=r1dataZ[n].b;}
}
std::vector<double> rdx(DATA_LENGTH),rdy(DATA_LENGTH),rdz(DATA_LENGTH),rdx2(DATA_LENGTH),rdy2(DATA_LENGTH),rdz2(DATA_LENGTH);
std::vector <double> vel_avrg_result(3);
std::vector <double> result(3);
double velintX[DATA_LENGTH];
double velintY[DATA_LENGTH];
double velintZ[DATA_LENGTH];
void count(){//% 频域积分double ts=0.02;//velint = iomega(acc, ts, 3, 2);iomega( ts, 3, 2);resfft1();//velint = detrend(velint);//ROS_INFO("----------------");for(int i=0;i<DATA_LENGTH;i++){velintX[i]=r1dataX[i].b;velintY[i]=r1dataY[i].b;velintZ[i]=r1dataZ[i].b;//ROS_INFO("%d :%lf %lf %lf",i,velintX[i],velintY[i],velintZ[i]);}//detrend(velintX,1);//detrend(velintY,1);//detrend(velintZ,1);for(int i=0;i<DATA_LENGTH;i++){vel_avrg_result[0]+=velintX[i];vel_avrg_result[1]+=velintY[i];vel_avrg_result[2]+=velintZ[i];//ROS_INFO("%d :%lf %lf %lf",i,vel_avrg_result[0],vel_avrg_result[1],vel_avrg_result[2]);}
//vel_avrg_result[0]/=DATA_LENGTH;
//vel_avrg_result[1]/=DATA_LENGTH;
//vel_avrg_result[2]/=DATA_LENGTH;
//ROS_INFO("%d :%lf %lf %lf",100,vel_avrg_result[0],vel_avrg_result[1],vel_avrg_result[2]);//disint = iomega( ts, 3, 1);iomega( ts, 3, 1);resfft1();res12v();ROS_INFO("contok");//% 去除位移中的二次项//p = polyfit(t, disint, 2);rdx=polyfit(dxa, dxb, 2);rdy=polyfit(dya, dyb, 2);rdz=polyfit(dza, dzb, 2);//disint = disint - polyval(p, t);
res12v();rdx2=polyval(rdx,dxa);rdy2=polyval(rdy,dya);rdz2=polyval(rdz,dza);for(int i=0;i<DATA_LENGTH;i++){rdx[i]-=rdx2[i];rdy[i]-=rdy2[i];rdz[i]-=rdz2[i];}result[0]=rdx[15];result[1]=rdy[15];result[2]=rdz[15];//return result;*/
}
// 一维滤波器信息结构体
typedef struct{double filterValue; //k-1时刻的滤波值，即是k-1时刻的值double kalmanGain; // Kalamn增益double A; // x(n)=A*x(n-1)+u(n),u(n)~N(0,Q)double H; // z(n)=H*x(n)+w(n),w(n)~N(0,R)double Q; //预测过程噪声偏差的方差double R; //测量噪声偏差，(系统搭建好以后，通过测量统计实验获得)double P; //估计误差协方差
} KalmanInfo;
/**
* @brief Init_KalmanInfo 初始化滤波器的初始值
* @param info 滤波器指针
* @param Q 预测噪声方差 由系统外部测定给定
* @param R 测量噪声方差 由系统外部测定给定
*/
void Init_KalmanInfo(KalmanInfo* info, double Q, double R)
{info->A = 1; //标量卡尔曼info->H = 1; //info->P = 10; //后验状态估计值误差的方差的初始值（不要为0问题不大）info->Q = Q; //预测（过程）噪声方差 影响收敛速率，可以根据实际需求给出info->R = R; //测量（观测）噪声方差 可以通过实验手段获得info->filterValue = 0;// 测量的初始值
}
double KalmanFilter(KalmanInfo* kalmanInfo, double lastMeasurement)
{//预测下一时刻的值double predictValue = kalmanInfo->A* kalmanInfo->filterValue; //x的先验估计由上一个时间点的后验估计值和输入信息给出，此处需要根据基站高度做一个修改//求协方差kalmanInfo->P = kalmanInfo->A*kalmanInfo->A*kalmanInfo->P + kalmanInfo->Q; //计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+qdouble preValue = kalmanInfo->filterValue; //记录上次实际坐标的值//计算kalman增益kalmanInfo->kalmanGain = kalmanInfo->P*kalmanInfo->H / (kalmanInfo->P*kalmanInfo->H*kalmanInfo->H + kalmanInfo->R); //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)//修正结果，即计算滤波值kalmanInfo->filterValue = predictValue + (lastMeasurement - predictValue)*kalmanInfo->kalmanGain; //利用残余的信息改善对x(t)的估计，给出后验估计，这个值也就是输出 X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))//更新后验估计kalmanInfo->P = (1 - kalmanInfo->kalmanGain*kalmanInfo->H)*kalmanInfo->P;//计算后验均方差 P[n|n]=(1-K[n]*H)*P[n|n-1]return kalmanInfo->filterValue;
}
KalmanInfo klmx;
KalmanInfo klmy;
KalmanInfo klmz;using namespace boost::asio; //定义一个命名空间，用于后面的读写操作
using namespace std;
serial::Serial ser; //声明串口对象
unsigned char buf[1]; //定义字符串长度
unsigned char data[512];//
unsigned int dn=0;
unsigned int i=0,j=0;double pitch=0, roll=0, yaw=0;double ax = 0.0;double ay = 0.0;double az = 0.0;double tsx = 0.0;double tsy = 0.0;double tsz = 0.0;double tvx = 0.0;double tvy = 0.0;double tvz = 0.0;Vectordouble oawV;Quaternion worldQ; //[w,x,y,z]Vectordouble aV;//[x,y,z]Vectordouble gV;//[x,y,z]Vectordouble gVo;//[x,y,z]Vectordouble realV;void countGravity(Quaternion worldQ){gV.x=2.0*(worldQ.x*worldQ.z-worldQ.w*worldQ.y);gV.y=2.0*(worldQ.w*worldQ.x+worldQ.y*worldQ.z);gV.z=worldQ.w*worldQ.w-worldQ.x*worldQ.x-worldQ.y*worldQ.y+worldQ.z*worldQ.z;}void countRealAccel(short int awx, short int awy, short int awz){realV.x=((double)awx)-(gV.x)*4096.0;realV.y=((double)awy)-(gV.y)*4096.0;realV.z=((double)awz)-(gV.z)*4096.0;gVo.x = gV.x;gVo.y = gV.y;gVo.z = gV.z;oawV.x=(double)awx;oawV.y=(double)awy;oawV.z=(double)awz;};double gravity[3]={0.0,0.0,0.0};double linear_acceleration[3]={0.0,0.0,0.0};void onSensorChanged(double realx,double realy,double realz){// 在本例中，alpha 由 t / (t + dT)计算得来，// 其中 t 是低通滤波器的时间常数，dT 是事件报送频率float alpha = 0.8;// 用低通滤波器分离出重力加速度gravity[0] = alpha * gravity[0] + (1 - alpha) * realx;gravity[1] = alpha * gravity[1] + (1 - alpha) * realy;gravity[2] = alpha * gravity[2] + (1 - alpha) * realz;// 用高通滤波器剔除重力干扰linear_acceleration[0] = realx - gravity[0];linear_acceleration[1] = realy - gravity[1];linear_acceleration[2] = realz - gravity[2];aV.x=linear_acceleration[0];aV.y=linear_acceleration[1];aV.z=linear_acceleration[2];}double acc2v(short int aw){return double (((double) aw)/4096.0)*9.8;}double acc2v(double aw){return double ((aw)/4096.0)*9.8;}ros::Time last_time;unsigned int startN=100;tf::Quaternion lastQ;void pulishMSG( short int awx, short int awy, short int awz,float yy,float pp,float rr,float qw,float qx,float qy,float qz,tf::TransformBroadcaster odom_broadcaster,ros::Publisher odom_pub){ros::Time current_time= ros::Time::now();double dt=(current_time.toSec()-last_time.toSec());last_time=current_time;worldQ.w=double(qw);worldQ.x=double(qx);worldQ.y=double(qy);worldQ.z=double(qz);worldQ.normalize();countGravity(worldQ);countRealAccel(awx, awy,awz);//ROS_INFO("awx=%d awy=%d awz=%d ",awx,awy,awz);//ROS_INFO("realx=%lf realy=%lf realz=%lf ",realV.x,realV.y,realV.z);onSensorChanged( realV.x, realV.y, realV.z);//onSensorChanged( double(awx), double(awy), double(awz));//ROS_INFO("linearx=%lf lineary=%lf linearz=%lf ",linear_acceleration[0],linear_acceleration[1],linear_acceleration[2]);if(startN==0){aV.rotate(&worldQ);ax=acc2v(aV.x);ay=acc2v(aV.y);az=acc2v(aV.z);odataX[datan].a=((datan==0)?0.0:(odataX[datan-1].a+dt));odataY[datan].a=odataX[datan].a;odataZ[datan].a=odataX[datan].a;odataX[datan].b=ax;odataY[datan].b=ay;odataZ[datan].b=az;datan+=1;if(datan==DATA_LENGTH){//ROS_INFO("----------------");for(datan=0;datan<DATA_LENGTH;datan++){ROS_INFO("dax=%lf day=%lf daz=%lf ",odataX[datan],odataY[datan],odataZ[datan]);}datan=0;count();//std::vector <double> posi=count();ROS_INFO("posix=%lf posiy=%lf posiz=%lf ",result[0],result[1],result[2]);//tsx+= result[0];//tsy+= result[1];//tsz+= result[2];//tsx+= vel_avrg_result[0]*DATA_LENGTH*dt;//tsy+= vel_avrg_result[1]*DATA_LENGTH*dt;//tsz+= vel_avrg_result[2]*DATA_LENGTH*dt;//ROS_INFO("vx=%lf vy=%lf vz=%lf ",vel_avrg_result[0],vel_avrg_result[1],vel_avrg_result[2]);}//ax=KalmanFilter(&klmx, ax);ay=KalmanFilter(&klmy, ay);az=KalmanFilter(&klmz, az);tvx +=ax*dt;tvy +=ay*dt;tvz +=az*dt;}else{startN--;}//tsx= tvx;//tsy= tvy;//tsz= tvz;tsx+= tvx*dt;tsy+= tvy*dt;tsz+= tvz*dt;if(tsx*tsx+tsy*tsy+tsz*tsz>3*3){tsx= 0.0;tsy= 0.0;tsz= 0.0;}tvx*=0.9;tvy*=0.9;tvz*=0.9;/*ax*=0.9;ay*=0.9;az*=0.9;*///tf::Quaternion Q=tf::createQuaternionFromRPY(0.0,0.0,0.0);// ( qx, qy, qz, qw ) ;//Q.setEulerZYX(0,0,3.1415926/180);//Q.x=double(nqx);//Q.y=double(nqy);//Q.z=double(nqz);//Q.w=double(nqw);geometry_msgs::Quaternion odom_quat= tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);odom_quat.x=worldQ.x;//Q.getX();odom_quat.y=worldQ.y;//Q.getY();odom_quat.z=worldQ.z;//Q.getZ();odom_quat.w=worldQ.w;//Q.getW();//first, we'll publish the transform over tfgeometry_msgs::TransformStamped odom_trans;odom_trans.header.stamp = current_time;odom_trans.header.frame_id = "odom";odom_trans.child_frame_id = "base_link";odom_trans.transform.translation.x = tsx*10;odom_trans.transform.translation.y = tsy*10;odom_trans.transform.translation.z = tsz*10;odom_trans.transform.rotation = odom_quat;//send the transformodom_broadcaster.sendTransform(odom_trans);//next, we'll publish the odometry message over ROSnav_msgs::Odometry odom;odom.header.stamp = current_time;odom.header.frame_id = "odom";//set the positionodom.pose.pose.position.x = tsx*10;odom.pose.pose.position.y = tsy*10;odom.pose.pose.position.z = tsz*10;//odom_trans.transform.translation.z;odom.pose.pose.orientation = odom_quat;//set the velocityodom.child_frame_id = "base_link";odom.twist.twist.linear.x = tvx;odom.twist.twist.linear.y = tvy;odom.twist.twist.linear.z = tvz;odom.twist.twist.angular.z = 0.0;//publish the messageodom_pub.publish(odom);}
int main(int argc, char** argv){ros::init(argc, argv, "mpu6050_teapotPacket");ros::NodeHandle n;ros::Publisher odom_pub;tf::TransformBroadcaster odom_broadcaster;odom_pub = n.advertise<nav_msgs::Odometry>("odom", 1);Init_KalmanInfo(&klmx,0.5,0.1);Init_KalmanInfo(&klmy,0.5,0.1);Init_KalmanInfo(&klmz,0.5,0.1);io_service iosev;serial_port sp(iosev, "/dev/ttyUSB0"); //定义传输的串口sp.set_option(serial_port::baud_rate(115200));sp.set_option(serial_port::flow_control());sp.set_option(serial_port::parity());sp.set_option(serial_port::stop_bits());sp.set_option(serial_port::character_size(8));iosev.run();ros::Rate r(200.0);last_time=ros::Time::now();lastQ=tf::createQuaternionFromRPY(0.0,0.0,0.0);while(n.ok()){while(1){try{read (sp,buffer(buf));data[dn]=buf[0];dn++;if(dn>=64){break;}//ROS_INFO("%d", buf[0]);//打印接受到的字符串}catch (boost::system::system_error e){ROS_ERROR_STREAM("read err ");break;}}//ROS_INFO("read");float qw,qx,qy,qz;float yy,pp,rr;short int awx,awy,awz;unsigned char *pwx,*pwy,*pwz;unsigned char * pw,*px,*py,*pz;unsigned char * pyy,*ppp,*prr;unsigned char flag=0;//xieyi//{'$',l,x,x,y,y,z,z,w,w,0,0,'\r','\n'}for(i=0;i<(dn-1);){if((data[i])=='$'){unsigned char len=data[i+1];if((i+len)>dn)break;//data length is not okif((data[i+len-2]!='\r')||(data[i+len-1]!='\n')){i++;continue;//package error};////ROS_INFO("d0=%d",'$');if(len==26){pw=(unsigned char*)&qw;px=(unsigned char*)&qx;py=(unsigned char*)&qy;pz=(unsigned char*)&qz;*pw=data[i+2];*(pw+1)=data[i+3];*(pw+2)=data[i+4];*(pw+3)=data[i+5];*px=data[i+6];*(px+1)=data[i+7];*(px+2)=data[i+8];*(px+3)=data[i+9];*py=data[i+10];*(py+1)=data[i+11];*(py+2)=data[i+12];*(py+3)=data[i+13];*pz=data[i+14];*(pz+1)=data[i+15];*(pz+2)=data[i+16];*(pz+3)=data[i+17];pwx=(unsigned char*)&awx;pwy=(unsigned char*)&awy;pwz=(unsigned char*)&awz;*pwx=data[i+18];*(pwx+1)=data[i+19];*pwy=data[i+20];*(pwy+1)=data[i+21];*pwz=data[i+22];*(pwz+1)=data[i+23];flag=1;i+=25;}if(len==16){pyy=(unsigned char*)&yy;ppp=(unsigned char*)&pp;prr=(unsigned char*)&rr;*pyy=data[i+2];*(pyy+1)=data[i+3];*(pyy+2)=data[i+4];*(pyy+3)=data[i+5];*ppp=data[i+6];*(ppp+1)=data[i+7];*(ppp+2)=data[i+8];*(ppp+3)=data[i+9];*prr=data[i+10];*(prr+1)=data[i+11];*(prr+2)=data[i+12];*(prr+3)=data[i+13];flag=1;i+=15;}if(len==20){pw=(unsigned char*)&qw;px=(unsigned char*)&qx;py=(unsigned char*)&qy;pz=(unsigned char*)&qz;*pw=data[i+2];*(pw+1)=data[i+3];*(pw+2)=data[i+4];*(pw+3)=data[i+5];*px=data[i+6];*(px+1)=data[i+7];*(px+2)=data[i+8];*(px+3)=data[i+9];*py=data[i+10];*(py+1)=data[i+11];*(py+2)=data[i+12];*(py+3)=data[i+13];*pz=data[i+14];*(pz+1)=data[i+15];*(pz+2)=data[i+16];*(pz+3)=data[i+17];flag=1;i+=19;}}else{//waitbreak;}i+=1;}//move to 0//if(i>0){for(j=0;i<dn;i++,j++){data[j]=data[i];}dn=j;if(flag){//ROS_INFO("w=%f x=%f y=%f z=%f",qw,qx,qy,qz);pulishMSG(awx,awy,awz,yy,pp,rr, qw, qx, qy, qz,odom_broadcaster,odom_pub);}ros::spinOnce();r.sleep();}
}