标定eai二维雷达和Imu

//
// Created by cai on 19-10-14.
//
利用机器人匀速直线运动时，两者的相对位姿不会变，联立多次直线运动的速度方向求解相对位姿
缺点和不足，imu数据的时间没分配好，手移动机器人，有微小旋转，需要对角速度积分，Imu坐标系有点奇怪

#include "ros/ros.h"
#include <iostream>
#include "std_msgs/String.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/Imu.h"
#include <vector>
#include <string>
#include <math.h>
#include <thread>
using namespace std;

vector<sensor_msgs::LaserScan::ConstPtr> scanMsgList;
vector<sensor_msgs::Imu::ConstPtr> imuMsgList;


//是由laserScan判断当前是直线运动还是由imu判断当前是直线运动,当用激光雷达判断是直线运动的时候，将序列
//中第一个scanmsg的时间戳和最后scanmsg的时间戳中间的imu的数据的xy值的和取平均值
//有的激光点扫到的距离为0要去掉,不然影响结果,
void scanCallback(const sensor_msgs::LaserScan::ConstPtr &msg) {
    scanMsgList.push_back(msg);
//    uint64_t nsec = msg->header.stamp.toNSec();
//    uint64_t sec = msg->header.stamp.toSec();
//    cout << "scanMsgListsec=" << sec << endl;
//    cout << "scanMsgListnsec=" << nsec << endl;
//    __pid_t pid = getpid();
//    cout << "scanpid=" << pid << endl;
    int meglen = 10;
     if (scanMsgList.size() > meglen) {
       // if (false) {
        cout << "-----------------------------------------------"<<endl;
        //判断现在是否在直线运动
        //雷达每转４次取平均值吧
        //找到gap最小的
        float gap_min = 10;
        int i_gap_min = 0;
        float dis_gap_min = 0;
        for (int i = 0; i < 45; ++i) {
            int not0_a = 0,not0_b = 0;
            float dis_sum_a = 0,dis_sum_b  = 0;
            float dis_rev_a = 0,dis_rev_b  = 0;
            for (int j = 0; j < 8; ++j) {
                //0到360
                float x_a = scanMsgList[0]->ranges[i * 8 + j];
                if ( x_a!= 0) {
                    not0_a++;
                    dis_sum_a += x_a;
                }
                //360-720
                float x_b = scanMsgList[0]->ranges[i * 8 + j + 360];
                if ( x_b!= 0) {
                    not0_b++;
                    dis_sum_b += x_b;
                }
                dis_rev_a = dis_sum_a / not0_a;
                dis_rev_b = dis_sum_b / not0_b;
            }
            //---------------------------------------------------------------
            int not0_a2 = 0,not0_b2 = 0;
            float dis_sum_a2 = 0,dis_sum_b2  = 0;
            float dis_rev_a2 = 0,dis_rev_b2  = 0;
            for (int j = 0; j < 8; ++j) {
                //0到360
                float x_a2 = scanMsgList[meglen]->ranges[i * 8 + j];
                if ( x_a2!= 0) {
                    not0_a2++;
                    dis_sum_a2 += x_a2;
                }
                //360-720
                float x_b2 = scanMsgList[meglen]->ranges[i * 8 + j + 360];
                if ( x_b2!= 0) {
                    not0_b2++;
                    dis_sum_b2 += x_b2;
                }
                dis_rev_a2 = dis_sum_a2 / not0_a2;
                dis_rev_b2 = dis_sum_b2 / not0_b2;
            }
            float d_increate = abs(dis_rev_a2 - dis_rev_a);
            float d_reduce = abs(dis_rev_b2 - dis_rev_b);
            //在一次运动中检测出不平行的直线运动
            //cout << "d_increate=" << d_increate << endl;
            //cout << "d_reduce=" <<  d_reduce<< endl;
            //距离减少和增加的差值
            float gap = abs(abs(d_increate )- abs(d_reduce));
            if (gap < gap_min) {
                gap_min = gap;
                i_gap_min = i;
                dis_gap_min = d_increate;
            }
        }


            cout << "gap_min=" << gap_min << endl;
            cout << "i_gap_min=" << i_gap_min << endl;
            cout << "dis_gap_min=" << dis_gap_min << endl;
            if (gap_min < 0.015 && dis_gap_min > 0.15) {
                //是做直线运动
                cout << "linear"<<endl;
                //时间
                //开始时间
                uint64_t begin_lidar_nsec = scanMsgList[0]->header.stamp.toNSec();
                //结束时间 
                uint64_t end_lidar_nsec = scanMsgList[meglen]->header.stamp.toNSec();
                cout << "begin_lidar_sec=" << begin_lidar_nsec << endl;
                cout << "end_lidar_sec=" << end_lidar_nsec << endl;

                //结束时间
                uint64_t time = end_lidar_nsec-begin_lidar_nsec;
                int64_t n1time = (int64_t) time;
                double n2time = (double) n1time;
                cout << "time=" << time << endl;
                cout << "n1time=" << n1time << endl;
                cout << "n2time=" << n2time << endl;
                //求速度的模
                float vec = dis_gap_min*1000000000ul/n2time;

                cout << "vec=" << vec << endl;
                //求速度的
                float theta = i_gap_min*4*(M_PI/180);

                cout << "cos(theta-180)=" << cos(theta-M_PI) << endl;
                cout << "sin(theta-180)=" << sin(theta-M_PI) << endl;
                float x = vec * cos(theta-M_PI);
                float y = vec * sin(theta-M_PI);
                cout << "theta=" << theta << endl;
                cout << "x=" << x << endl;
                cout << "y=" << y << endl;
                //得到imu
                //得到时间在激光雷达运动时间内的
                int imu_begin_i = -1;
                int imu_end_i = -1;
                for (int i = 0; i < imuMsgList.size(); ++i) {
                    cout << "imuMsgList[i]->header.stamp.toNSec()=" << imuMsgList[i]->header.stamp.toNSec() << endl;
                    if ((imuMsgList[i]->header.stamp.toNSec() > begin_lidar_nsec)) {
                        imu_begin_i = i;
                        break;
                    }
                }
                for (int j = imuMsgList.size()-1; j >-1; --j) {
                    cout << "imuMsgList[j]->header.stamp.toNSec()=" << imuMsgList[j]->header.stamp.toNSec() << endl;
                    if ((imuMsgList[j]->header.stamp.toNSec() < end_lidar_nsec) ) {
                        imu_end_i = j;
                        break;
                    }
                }
                cout << "imu_end_i-imu_begin_i+1=" << imu_end_i-imu_begin_i+1 << endl;
                //陀螺仪的是加速度,假设每个加速度作用的时间区间一样大,平分时间
                //累积的速度
                float accum_vec_x = 0;
                float accum_vec_y= 0;
                float accum_vec = 0;
                //累积的距离
                float accum_dis_x = 0;
                float accum_dis_y = 0;
                float accum_dis = 0;
                //总时间
                //开始时间
                uint64_t begin_imu_nsec = imuMsgList[imu_begin_i]->header.stamp.toNSec();
                //结束时间
                uint64_t end_imu_nsec = imuMsgList[imu_end_i]->header.stamp.toNSec();
                cout << "begin_imu_nsec=" << begin_imu_nsec << endl;
                cout << "end_imu_nsec=" << end_imu_nsec << endl;
                //结束时间
                uint64_t time_imu = end_imu_nsec-begin_imu_nsec;
                int64_t n1time_imu = (int64_t) time_imu;
                double imu_sum_time = ((double) n1time_imu)/1000000000ul;
                cout << "imu_sum_time=" << imu_sum_time << endl;
                //平均时间
                float imu_rev_time = imu_sum_time/(imu_end_i-imu_begin_i+1);
                for (int k = imu_begin_i; k < imu_end_i+1; ++k) {
                    float xa = imuMsgList[k]->linear_acceleration.x;
                    float ya =  imuMsgList[k]->linear_acceleration.y;
                    //cout << "xa=" << xa << endl;
                   // cout << "ya=" << ya << endl;
                    //先分别求x,y,  再结合
                    //累积距离
                    accum_dis_x = accum_dis_x + (accum_vec_x * imu_rev_time )+ (0.5 * xa * pow(imu_rev_time, 2));
                    accum_dis_y = accum_dis_y + (accum_vec_y * imu_rev_time )+ (0.5 * ya * pow(imu_rev_time, 2));
                    //累积速度
                    accum_vec_x = accum_vec_x + xa*imu_rev_time;
                    accum_vec_y = accum_vec_y + ya*imu_rev_time;
                    //z轴方向没怎么分去加速度
                    //cout << "imuMsgList[k]->linear_acceleration.z=" << imuMsgList[k]->linear_acceleration.z << endl;
                }
                cout << "accum_dis_x=" << accum_dis_x << endl;
                cout << "accum_dis_y=" << accum_dis_y << endl;
                cout << "accum_vec_x=" << accum_vec_x << endl;
                cout << "accum_vec_y=" << accum_vec_y << endl;
                //去除静止时的误差
                //accum_vec_x -= -0.012;
               // accum_vec_y -= 0.023;
                //不旋转情况下,两者的模长应该是一致的
                float movec = sqrt(pow(accum_vec_x, 2)+pow(accum_vec_y,2));
                float modis = sqrt(pow(accum_dis_x, 2)+pow(accum_dis_y,2));
                cout << "movec=" << movec << endl;
                cout << "modis=" << modis << endl;
                //平均速度
                float imu_rev_vec = modis / imu_sum_time;
                cout << "imu_rev_vec=" << imu_rev_vec << endl;
            } else{
                //cout << "no linear"<<endl;
            }
        scanMsgList.clear();
        imuMsgList.clear();
        }

    }

//imu x方向上静止的平均误差,总是负的,-0.012
float imu_rev_err_x = 0;
int err_x = 0;
//imu y方向上的静止平均误差，总是正的,静止时大概是0.023
float imu_rev_err_y = 0;
int err_y = 0;

void imuCallback(const sensor_msgs::Imu::ConstPtr &msg) {

    imuMsgList.push_back(msg);
//    uint64_t nsec = msg->header.stamp.toNSec();
//    uint64_t sec = msg->header.stamp.toSec();
//    cout << "imuMsgListsec=" << sec << endl;
//    cout << "imuMsgListnsec=" << nsec << endl;
//    __pid_t pid = getpid();
//    cout << "Imupid=" << pid << endl;
     if (imuMsgList.size() > 15) {
         // if (false) {
        //累积的速度
        float accum_vec_x = 0;
        float accum_vec_y= 0;
        float accum_vec = 0;
        //累积的距离
        float accum_dis_x = 0;
        float accum_dis_y = 0;
        float accum_dis = 0;
        //开始时间
        uint64_t begin_imu_nsec = imuMsgList[0]->header.stamp.toNSec();
        //结束时间
        uint64_t end_imu_nsec = imuMsgList[15]->header.stamp.toNSec();
        cout << "begin_imu_nsec=" << begin_imu_nsec << endl;
        cout << "end_imu_nsec=" << end_imu_nsec << endl;
        //结束时间
        uint64_t time_imu = end_imu_nsec - begin_imu_nsec;
        int64_t n1time_imu = (int64_t) time_imu;
        double imu_sum_time = ((double) n1time_imu) / 1000000000ul;
        cout << "imu_sum_time=" << imu_sum_time << endl;
        //平均时间
        float imu_rev_time = imu_sum_time / (16);
        for (int k = 0; k < 16; ++k) {
            float xa = imuMsgList[k]->linear_acceleration.x;
            float ya = imuMsgList[k]->linear_acceleration.y;
            cout << "xa=" << xa << endl;
            cout << "ya=" << ya << endl;
            //先分别求x,y,  再结合
            //累积距离
            accum_dis_x = accum_dis_x + (accum_vec_x * imu_rev_time) + (0.5 * xa * pow(imu_rev_time, 2));
            accum_dis_y = accum_dis_y + (accum_vec_y * imu_rev_time) + (0.5 * ya * pow(imu_rev_time, 2));
            //累积速度
            accum_vec_x = accum_vec_x + xa * imu_rev_time;
            accum_vec_y = accum_vec_y + ya * imu_rev_time;
            //z轴方向没怎么分去加速度
            //cout << "imuMsgList[k]->linear_acceleration.z=" << imuMsgList[k]->linear_acceleration.z << endl;
        }
        err_x++;
        imu_rev_err_x = (imu_rev_err_x*(err_x-1) + accum_vec_x) / err_x;
        err_y++;
        imu_rev_err_y = (imu_rev_err_y *(err_y-1)+ accum_vec_y )/ err_y;
        cout << "accum_dis_x=" << accum_dis_x << endl;
        cout << "accum_dis_y=" << accum_dis_y << endl;
        cout << "accum_vec_x=" << accum_vec_x << endl;
        cout << "accum_vec_y=" << accum_vec_y << endl;
        cout << "imu_rev_err_x=" << imu_rev_err_x << endl;
        cout << "imu_rev_err_y=" << imu_rev_err_y << endl;
        //不旋转情况下,两者的模长应该是一致的
        float movec = sqrt(pow(accum_vec_x, 2) + pow(accum_vec_y, 2));
        float modis = sqrt(pow(accum_dis_x, 2) + pow(accum_dis_y, 2));
        cout << "movec=" << movec << endl;
        cout << "modis=" << modis << endl;
        //平均速度
        float imu_rev_vec = modis / imu_sum_time;
        cout << "imu_rev_vec=" << imu_rev_vec << endl;
        scanMsgList.clear();
        imuMsgList.clear();
    }

}

int main(int argc, char **argv) {
    string nodeName = "getData_node";
    string topicName_scan = "scan";
    string topicName_imu = "imu_data";

    //初始化节点
    ros::init(argc, argv, nodeName);
    ros::NodeHandle node;
    //创建订阅者
    const ros::Subscriber &subscriber_imu_data = node.subscribe(topicName_imu, 1000, imuCallback);
    const ros::Subscriber &subscriber_scan = node.subscribe(topicName_scan, 1000, scanCallback);
    // 阻塞线程
    ros::spin();
    return 0;
}