项目实训 - 智能车系统 - 第三周记录

项目实训 - 智能车系统 - 第三周记录

日期：3.07 – 3.13

项目进度

本周工作进展：

• 继续进行话题的移植工作
• 给可视化部分提供了测试用的接口

1、imageProjection中发布的lio_sam/feature/cloud_info话题的移植

上周将featureExtraction中发布的话题移植完（发布了三个话题，但是只有一个被用到）。本周考虑移植image Projection中的话题。

imageProjection中发布话题如下：

• lio_sam/deskew/cloud_info --> feature进程中
• lio_sam/deskew/cloud_info --> rviz中 （这个暂时不改

该话题的移植工作和前一个话题的移植过程类似。

在按照步骤移植完成后，发现该话题与上一个话题之间是有联系的：

imageProjection –lio_sam/deskew/cloud_info–> featureExtraction –lio_sam/feature/cloud_info–> mapOptmization

所以当这两个话题移植完成后，在流向图里，featureExtraction节点会变得“孤立”：

所以相应的，需要将featureExtraction中的与ros有关的中间变量进行修改，这里不做过多赘述。

过程中一些细节记录：

sub.getNumSubscribers() 获取订阅该话题的节点个数

• 用来判断是否有节点订阅该话题，从而判断是否发布

https://blog.csdn.net/zhanghm1995/article/details/104224403

2、ros原生函数、项目自定义函数重写

pcl/pcl.h

    template<typename T>
    void toROSMsg(const pcl::PointCloud<T> &pcl_cloud, sensor_msgs_PointCloud2 &cloud, sensor_msgs_PointField fields[], uint8_t data[])
    {
        pcl::PCLPointCloud2 pcl_pc2;
        pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);

        moveFromPCL(pcl_pc2, cloud, fields,data); //
    }
...

rosFunc.h

//重写一个，适配stamp是自定义类型
sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud<PointType>::Ptr thisCloud, msgs_Time thisStamp, std::string thisFrame)
{
    sensor_msgs::PointCloud2 tempCloud;
    pcl::toROSMsg(*thisCloud, tempCloud);
    // tempCloud.header.stamp = thisStamp;
    tempCloud.header.stamp.nsec = thisStamp.nsec;
    tempCloud.header.stamp.sec = thisStamp.sec;
    tempCloud.header.frame_id = thisFrame;
    if (thisPub->getNumSubscribers() != 0)
        thisPub->publish(tempCloud);
    return tempCloud;
}

3、给可视化部分提供了测试用的接口

在3D点云图的可视化中，需要由我们的项目提供点云数据。

这个过程在项目中本来的流程是：rviz订阅若干项目发布的话题，根据这些数据进行3D重建。

我们的做法是利用pcl来自己实现上述过程（由我们组的朱庆杰同学主要负责），由于真车还没有到，所以需要先用rosbag中的record功能进行数据的录制，然后由一个转换程序将rosbag play回访的数据包中的ros类型转换为自定义消息类型，朱同学接收到这些消息后就可以进行后续的处理。

程序如下：

主要部分为pubTopic节点，接收数据包中的ros数据，转换成自定义类型后发布。

//核心代码：
        /**
     * @brief 回调函数
     * 其余两个回调函数类似
     * @param msgIn 
     */
    void subCloudRegisteredRawHandler(const sensor_msgs::PointCloud2ConstPtr& msgIn)
    {
        
        sensor_msgs::PointCloud2 pointCloud2 = *msgIn;
        sensor_msgs_PointField fields[pointCloud2.fields.size()];
        uint8_t data[pointCloud2.data.size()];

        cloud_registered_raw.fields = fields;
        cloud_registered_raw.data = data;

        // 类型转换
        change_sensor_msgs_PointCloud2(pointCloud2, cloud_registered_raw);


        //传输
        memcpy(pubCloudRegisteredRaw_sharemem_fields->point, (void*)cloud_registered_raw.fields, (int) cloud_registered_raw.fields_number * sizeof(sensor_msgs_PointField));
        memcpy(pubCloudRegisteredRaw_sharemem_data->point, (void*)cloud_registered_raw.data,  (int) cloud_registered_raw.data_number * sizeof(uint8_t));
      
        addMessage_sensor_msgs_PointCloud2((sensor_msgs_PointCloud2_List*)pubCloudRegisteredRaw_sharemem->point, cloud_registered_raw);

    }

技术难点

基于mmap通信框架的通信机制的搭建

ros原生函数的重写

bug记录

bug1：指针的声明周期问题

该问题出现在调用自定义的函数toROSMsg中，该函数是重写的pcl库中与ros有关的部分，将一个sensor_msgs::PointCloud2转换为pcl::PointCloud 类型。

该重写时，需要将原本的vector类型变成数组，在这个过程中，我开始将数组开在函数中，这样会造成当函数返回时，数组被回收，从而使数据丢失。

所以后续要注意指针数组的生命周期问题

（虽然bug描述很简单，但是找出这个bug真的花了好久leimu）

bug2：g++ o3优化后存在的问题

问题代码：

int number = 0;
//接收信息
volatile int last_number = 0;
volatile int now_pos = 0;
volatile int now_number = 0;

while(true) {


    now_number = queue_cloud_info->count;
    now_pos = queue_cloud_info->now;


    if(number == 0) std::cout<<now_number<<'f';
    number++;

    if(now_number != last_number) {
        //ROS_INFO("start sub!!!!");
            //std::cout<<"sub start 13"<<endl;   
        // 说明有数据更新了
                last_number = now_number;

注意第17行的if语句，如果没有14-15行的打印语句，则无论如何都不会进入这个if（是不是很离谱）

通过和队友的讨论后，我们认为是O3优化后，发现now_number在本程序内会一直等于0（因为实际的值是由其他进程通过共享内存提过，只看本进程雀氏一直为0），所以直接优化掉if里的代码。

我们通过反汇编的方式分别得到了o1和o3优化对应的.s文件，通过分析后确实是对应jne语句不见了，并且now_number也被优化掉。

于是我们尝试了在O3优化的基础上添加volatile关键字、以及添加命令组织某个代码段被优化等，都没有效果。

所以这里暂时通过打印now_number的方式使得该变量不会被优化掉。

其他

本周去了千佛山一趟，看了下小车，拷贝回来了相关的数据包

参考资料：

rosPcl相关 http://docs.ros.org/en/hydro/api/pcl/html/index.html