lio-sam实车调试

Posted 2021-12-28 哔哔啵嘎嘎

以下是些没些营养的碎碎念，作为理清思路用！

1.发现缺失map->odom->base_link坐标变换，使用rqt查看正常的tf树：

 发现map->odom->base_link由imupreintegration模块发出，odom->lidar_link由mapOptimization模块发出。

排除应该不是imu频率的问题；

1.发现是没有做第一次优化：也就是doneFirstOpt始终为 false

void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg)

void imuHandler(const sensor_msgs::Imu::ConstPtr &imu_raw)

        std::lock_guard<std::mutex> lock(mtx);


        sensor_msgs::Imu thisImu = imuConverter(*imu_raw); // 将imu信息转换到雷达坐标系下表达,其实也就是获得雷达运动的加速度、角速度和姿态信息

        imuQueOpt.push_back(thisImu);
        imuQueImu.push_back(thisImu);

        if (doneFirstOpt == false)
            return;

2.怀疑没进入这个函数：也就是没有"lio_sam/mapping/odometry_incremental"话题发布出来（由mapOptimization的pubLaserOdometryIncremental发布出来）

void odometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg)

void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr &msgIn)

3.发现这个回调函数laserCloudInfoHandler也没进去，订阅的是lio_sam/feature/cloud_info，现在查一下话题发布出来了吗？

-没发布出来

4.擦，publishClouds()没进去，所以lio_sam/deskew/cloud_info没发出来！

5.擦！找到原因了！

 if (!deskewInfo())
            ROS_INFO("deskew_info failed");
            return;
        

原来是畸变矫正失败了。

6.畸变矫正为什么会失败呢？猜测可能是雷达的时间戳和imu的不一样。写一个程序测一下

7.妈呀！发现imu数据没存进来，证据

void imuDeskewInfo()
    
        cloudInfo.imuAvailable = false;

        while (!imuQueue.empty())
        
            ROS_INFO("ImuQueue is not empty");
            // 丢弃早于当前雷达帧开始扫描时间戳的缓存的imu帧
            if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01) 
                imuQueue.pop_front();
            
            else
                break;
        

8.并不是imu没存进来，而是他们时间戳不一致！！！！气死了，看看这神奇的时间戳，imu的频率100hz,间隔0.01s,雷达频率为10hz，间隔0.1s，看来imu的时间以毫秒计的。

258216.000000000
258226.000000000
258236.000000000
258246.000000000
258256.000000000
258266.000000000
258276.000000000
258286.000000000
258296.000000000
258306.000000000
258316.000000000
258326.000000000
258336.000000000
258346.000000000
258356.000000000
258366.000000000
258376.000000000

1638538243.60303759574890
1638538243.70307374000549
1638538243.80305027961731
1638538243.90302062034607
1638538244.00300979614258
1638538244.10288214683533
1638538244.20300960540771
1638538244.30299758911133
1638538244.40297722816467
1638538244.50284433364868
1638538244.60297465324402
1638538244.70297312736511
1638538244.80297017097473
1638538244.90284919738770
1638538245.00297260284424
1638538245.10283350944519
1638538245.20288324356079

9.尝试打ros时间戳，失败，应该是回调函数缓冲区的问题。但为啥lidar的这么标准呢！

绝对是消息队列缓存的问题，多线程执行两个回调，执行lidar线程的时候正好缓存了4-5个imu数据，所以可以看到imu数据是以4个时间戳为间隔的!!!!!！！！

1638627183.601892528
1638627183.601977226
1638627183.602121231
1638627183.602135432
1638627183.641787980
1638627183.641811215
1638627183.641958043
1638627183.642166612
1638627183.671778537
1638627183.671821032
1638627183.671833001
1638627183.711852126
1638627183.712271755
1638627183.712285334
1638627183.712289181
1638627183.751722835
1638627183.751833635
1638627183.751912254
1638627183.781814012
1638627183.781834979
1638627183.781974295
1638627183.812083508
1638627183.812150075
1638627183.812162406
1638627183.812169311
//-----------------
1638627183.59612226486206
1638627183.69610762596130
1638627183.79492211341858
1638627183.89474797248840
1638627183.99510645866394
1638627184.09764385223389
1638627184.19502854347229
1638627184.29512143135071
1638627184.39541459083557
1638627184.49207997322083
1638627184.59496808052063
1638627184.69513773918152
1638627184.79462718963623
1638627184.89461898803711
1638627184.99531221389771
1638627185.09141755104065
1638627185.19279575347900
1638627185.29200410842896

了解到车端传感器时间同步，pps用作相机和雷达，雷达用IEEE1588协议。

 突然发现可以在imu发布的时候，打上时间戳！

2.尝试message_filters时间同步的办法，失败了

--------------------------------------------------

最终通过在INS端打ros时间戳的方法解决了，十分舒适