瞎忙好几天,草草做个总结,贴上混乱代码一堆 涉及内容如下
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 quaternion
mpu.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 Permutation
int 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 - 1
arg = - 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 [] 中的下标为 t
treal = 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 - 1
arg = 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 [] 中的下标为 t
treal = 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 algorithm
if (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)i
dataX[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)+q
double 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 tf
geometry_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 transform
odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//set the position
odom.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 velocity
odom.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 message
odom_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 ok
if((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{
//wait
break;
}
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();
}
}