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

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

日期：2.28 – 3.06

项目进度

本周工作进展：

• 通信框架的完善及完成
• 完成了第一个话题的移植

1、通信框架选择

lio-sam项目自身是基于ros的项目，进程间使用的通信机制主要是ros中的topic话题通信机制。该机制基于socket网络通信，与ros系统紧密结合，无法在原生linux系统中使用。所以制定一套新的通信机制是移植工作的一项重要内容。

在寒假的时候，经过我和组内罗源同学的学习交流后，最终决定使用mmap通信框架来指定通信机制。mmap通信框架有如下几个有点：

• 与socket相比，mmap的使用更加便捷，而且是基于共享内存的通信方式，传输速度比socket快，而且学习和使用成本更低。
• 与shm相比，mmap不需要自己定义锁和互斥量。

总而言之，mmap简单好用，所以选择了mmap。

2、通信机制构建

mmap的机制，简而言之就是通过创建文件的方式来进行内存共享，用来映射的文件作为共享内存区。

我将与通信相关的源文件统一放在communicator.h文件夹下，结构如下：

files文件夹

从来存储上文提到的映射文件

communicator.h

最底层的通信接口，包括创建共享内存、连接共享内存、释放共享内存等。

所以接口见下：

/**
 * @brief used to define share memory
 * @param fd：共享内存的句柄
 * @param point：共享内存区域首地址
 * 
 */
struct MemoryDefinition
{
    int fd;
    void * point;
};

/**
 * @brief Create a Share Memory, and return a MemoryDefinition pointer.
 * 
 * @param fileName：共享内存的映射文件的名称
 * @param size：共享内存文件大小
 * @return MemoryDefinition* 
 */
MemoryDefinition* createShareMemory(const char* fileName, size_t size);


/**
 * @brief Connect a Share Memory, and return a MemoryDefinition pointer.
 * 
 * @param fileName 映射文件名称
 * @param size 文件大小
 * @return MemoryDefinition*  
 */
MemoryDefinition* connectShareMemory(const char* fileName, size_t size);

/**
 * @brief unmap between the noraml file and the share memory
 * 
 * @param definition a MemoryDefinition object used to describe a specific share memory
 */
void unMap(const MemoryDefinition* definition, size_t size);

message.h

用于向共享内存中写入具体的数据。

struct std_msgs_Header_List
{
    int now; // the index of the current message
    int next;
    int count; // the number of all the message
    std_msgs_Header content[LIST_LENGTH];//有没有什么办法复用？
};


void addMessage_std_msgs_Header(std_msgs_Header_List* theList, std_msgs_Header& theMessage)
{
    int theIndex = theList->next;
    theList->content[theIndex] = theMessage;
    theList->now = theList->next;
    theList->next = (theIndex + 1) % LIST_LENGTH;
    theList->count++;
}

msg2struct.h

考虑到ros中自定义了许多消息类型，如std_msgs::Header、sensor_msgs::PointCloud2等类型，用于topic话题的传递。考虑到移植工作的可操作性，我们用结构体的方式重写了ros中的部分消息类型（lvi-sam项目中用到的消息类型），用于我们后续的消息传递。

namespace rosMsgs
{
    struct msgs_Time
    {
        int32_t sec;
        int32_t nsec;
    };
    struct msgs_duration
    {
        int32_t sec;
        int32_t nsec;
    };

    struct std_msgs_Header
    {
        uint32_t seq;
        msgs_Time stamp;
        char frame_id[STRING_LENGTH];
    };
}
.
.
.

msgs_init.h

这是第二周之后新加入的，等到后面再细说。

3、通信机制有关指针的完善

在进行了基本通信框架的构建后，还有一个比较严重的问题：在ros的消息类型中，有的类型有vector成员变量。在经过我们的测试后，发现mmap通信方式只能进行基本数据类型的传递，不能传递vector。所以我考虑用指针替代vector，这样就会出现一个问题：在进程间通信时，指针指向的数据无法直接与源数据一起被传输，需要进行额外的操作。

针对上述情况，我的解决方法是：

• 首先在strcut的定义中，为每个指针维护一个变量，用来记录指针指向数组的大小（eg：char* data，会同时维护一个int data_number记录data数组的大小）
• 在为原数据创建映射文件作为共享内存的同时，为原数据中的指针指向的数组也创建映射文件，在传输时直接将对应数组拷贝到共享内存区中
• 为每个指针数组人为规定一个上界，用来进行共享文件初始大小的确定

上面的过程可以用memcpy函数来实现。

到此为止，项目的通信架构已经确定完成，可以进行实际的移植工作了

4、featureExtraction中发布的lio_sam/feature/cloud_info话题的移植

针对具体的移植，我的做法是：

• 以每一个话题为中心，将发布者进程的pub过程和接收者进程的sub过程进行重写，将ros的部分复写一份自定义的通信机制
• 确定传输的数据无误后，剔除ros的通信部分，用自定义通信来代替，然后运行整个项目，看是否能成功运行

具体的流程可以概括为如下步骤（在移植过程中总结出来的）：

1. 在communication_LVI_SAM/files文件夹中创建属于要修改的文件的映射文件的文件夹（属于话题发布者）
2. 在文件夹中创建对应的映射文件
3. 在message.h文件中添加对应的接口
4. define消息类型中指针指向的数组的最大长度（后续步骤8时需要根据实际进行修改）
5. 定义全局映射文件变量
6. 在初始化构造函数的时候，对映射文件进行初始化
7. 修改.cpp文件中所有publish的部分，在ros发布的同时，同时进行我们自定义的发布
   1. publisher的同时进行自定义的发布
      1. 修改的时候注意一些ROS类型的修改（即对pub所在的sub函数中ros相关的修改）
   2. 接收端重写一个多线程（std::Thread），以及对应的函数
   3. 接收对应的消息
   4. 进行消息的连接（指针）
   5. 对ROS的相关的函数进行修改
8. 测试每个数组数据的上限

需要注意的是，针对ros接收端的回调函数，我们使用std::thread多线程来进行替代。

关键代码展示（部分）：

featureExtraction.cpp 发布端

/** 
 * @brief 定义指针对应数组的最大长度
 * 发布端
 */
#define MAX_lio_sam_feature_cloud_info_startRingIndex 100
#define MAX_lio_sam_feature_cloud_info_endRingIndex 100
#define MAX_lio_sam_feature_cloud_info_pointColInd 100
#define MAX_lio_sam_feature_cloud_info_pointRange 100
#define MAX_lio_sam_feature_cloud_info_cloud_deskewed_fields 100
#define MAX_lio_sam_feature_cloud_info_cloud_deskewed_data 800000
...
    
/**
 * @brief 定义映射文件变量
 * MemoryDefinition
 * 发布端
 */
MemoryDefinition* pubLaserCloudInfo_sharemem;
MemoryDefinition* pubCornerPoints_sharememe;
MemoryDefinition* pubSurfacePoints_sharemem;
...
    
/**
 * @brief 创建映射文件
 * 发布端
 */
pubLaserCloudInfo_sharemem = createShareMemory("/home/cislc2019/catkin_ws/src/LIO-SAM-master/src/communication_LVI_SAM/files/featureExtraction_files/lio_sam-feature-cloud_info", sizeof(lio_sam_msgs_cloud_info_List));
...
    
/**
 * @brief 同时用自定义的方式发布消息
 * 将ros类型定义的消息赋值给自定义类型
 * 后续应该把这些定义成接口
 * 接口：
 * 传入参数：原始的ros消息类型，对应的自定义消息类型
 * 
 * 然后进行发布
 * 
 */
lio_sam_msgs_cloud_info cloudInfo_sharemem = cloudInfo;
// 指针对应数组
/**
 * @brief 测试出每个指针数组的上界
 * 多次打印取最大值
 * 
 */
// ROS_INFO("*******************************************************************************8");
// ROS_INFO("MAX_lio_sam_feature_cloud_info_startRingIndex size: %d",cloudInfo.startRingIndex.size());
memcpy(pubLaserCloudInfo_sharemem_startRingIndex->point, (void*)cloudInfo.startRingIndex, cloudInfo.startRingIndex_number * sizeof(int32_t));
// ROS_INFO("MAX_lio_sam_feature_cloud_info_endRingIndex size: %d",cloudInfo.endRingIndex.size());
memcpy(pubLaserCloudInfo_sharemem_endRingIndex->point, (void*)cloudInfo.endRingIndex, cloudInfo.endRingIndex_number * sizeof(int32_t));
// ROS_INFO("MAX_lio_sam_feature_cloud_info_pointColInd size: %d",cloudInfo.pointColInd.size());
memcpy(pubLaserCloudInfo_sharemem_pointColInd->point, (void*)cloudInfo.pointColInd, cloudInfo.pointColInd_number * sizeof(int32_t));
// ROS_INFO("MAX_lio_sam_feature_cloud_info_pointRange size: %d",cloudInfo.pointRange.size());
memcpy(pubLaserCloudInfo_sharemem_pointRange->point, (void*)cloudInfo.pointRange, cloudInfo.pointRange_number * sizeof(float));

...


addMessage_lio_sam_msgs_cloud_info((lio_sam_msgs_cloud_info_List*)pubLaserCloudInfo_sharemem->point, cloudInfo_sharemem);
// pubLaserCloudInfo_sharemem


// pubLaserCloudInfo.publish(cloudInfo);

mapOptmization.cpp 接收端

std::thread subCloudThread(&mapOptimization::subCloudInfoThread, &MO);
...

void subCloudInfoThread()
{
    /**
     * @brief 重写的话题回调函数，接收自定义的消息类型
     * 步骤：
     * while死循环，不断的访问内存区，根据之前设置的标签判断是否是新消息
     * 
    */
    int number = 0;
    //标记作用
    volatile int last_number = 0;
    volatile int now_number = 0;
    volatile int now_pos = 0;

    lio_sam_msgs_cloud_info_List* queue_cloud_info = (lio_sam_msgs_cloud_info_List* ) subCloud_sharemem->point;
    while(true) {

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

            if(number == 0) std::cout<<now_number;
             number++;
        if(now_number != last_number) {

            // 说明有数据更新了
            last_number = now_number;
            lio_sam_msgs_cloud_info cloudInfo_sharemem;
            // cloudInfo_sharemem = queue_cloud_info->content[queue_cloud_info->now];
            cloudInfo_sharemem = queue_cloud_info->content[now_pos];


                // 指针数据 先获取出来再说
            int32_t cloudInfo_startRingIndex[(int) cloudInfo_sharemem.startRingIndex_number];
            int32_t cloudInfo_endRingIndex[(int) cloudInfo_sharemem.endRingIndex_number];
            int32_t cloudInfo_pointColInd[(int) cloudInfo_sharemem.pointColInd_number];
            float cloudInfo_pointRange[(int) cloudInfo_sharemem.pointRange_number];

            sensor_msgs_PointField cloudInfo_cloud_deskewed_fields[(int) cloudInfo_sharemem.cloud_deskewed.fields_number];
            uint8_t cloudInfo_cloud_deskewed_data[(int) cloudInfo_sharemem.cloud_deskewed.data_number];
            sensor_msgs_PointField cloudInfo_cloud_corner_fields[(int) cloudInfo_sharemem.cloud_corner.fields_number];
            uint8_t cloudInfo_cloud_corner_data[(int) cloudInfo_sharemem.cloud_corner.data_number];
            sensor_msgs_PointField cloudInfo_cloud_surface_fields[(int) cloudInfo_sharemem.cloud_surface.fields_number];
            uint8_t cloudInfo_cloud_surface_data[(int) cloudInfo_sharemem.cloud_surface.data_number];



            memcpy((void*)cloudInfo_startRingIndex, subCloud_sharemem_startRingIndex->point, (int) cloudInfo_sharemem.startRingIndex_number * sizeof(int32_t));
            memcpy((void*)cloudInfo_endRingIndex, subCloud_sharemem_endRingIndex->point, (int) cloudInfo_sharemem.endRingIndex_number * sizeof(int32_t));
            memcpy((void*)cloudInfo_pointColInd, subCloud_sharemem_pointColInd->point, (int) cloudInfo_sharemem.pointColInd_number * sizeof(int32_t));
            memcpy((void*)cloudInfo_pointRange, subCloud_sharemem_pointRange->point, (int) cloudInfo_sharemem.pointRange_number * sizeof(float));

            memcpy((void*)cloudInfo_cloud_deskewed_fields, subCloud_sharemem_cloud_deskewed_fields->point, (int) cloudInfo_sharemem.cloud_deskewed.fields_number * sizeof(sensor_msgs_PointField));
            memcpy((void*)cloudInfo_cloud_deskewed_data, subCloud_sharemem_cloud_deskewed_data->point, (int) cloudInfo_sharemem.cloud_deskewed.data_number * sizeof(uint8_t));
            memcpy((void*)cloudInfo_cloud_corner_fields, subCloud_sharemem_cloud_corner_fields->point, (int) cloudInfo_sharemem.cloud_corner.fields_number * sizeof(sensor_msgs_PointField));
            memcpy((void*)cloudInfo_cloud_corner_data, subCloud_sharemem_cloud_corner_data->point, (int) cloudInfo_sharemem.cloud_corner.data_number * sizeof(uint8_t));
            memcpy((void*)cloudInfo_cloud_surface_fields, subCloud_sharemem_cloud_surface_fields->point, (int) cloudInfo_sharemem.cloud_surface.fields_number * sizeof(sensor_msgs_PointField));
            memcpy((void*)cloudInfo_cloud_surface_data, subCloud_sharemem_cloud_surface_data->point, (int) cloudInfo_sharemem.cloud_surface.data_number * sizeof(uint8_t));


            //对指针进行赋值！
            cloudInfo_sharemem.startRingIndex = cloudInfo_startRingIndex;
            cloudInfo_sharemem.endRingIndex = cloudInfo_endRingIndex;
            cloudInfo_sharemem.pointColInd = cloudInfo_pointColInd;
            cloudInfo_sharemem.pointRange =cloudInfo_pointRange;

            cloudInfo_sharemem.cloud_deskewed.fields = cloudInfo_cloud_deskewed_fields;
            cloudInfo_sharemem.cloud_deskewed.data = cloudInfo_cloud_deskewed_data;
            cloudInfo_sharemem.cloud_corner.fields = cloudInfo_cloud_corner_fields;
            cloudInfo_sharemem.cloud_corner.data = cloudInfo_cloud_corner_data;
            cloudInfo_sharemem.cloud_surface.fields = cloudInfo_cloud_surface_fields;
            cloudInfo_sharemem.cloud_surface.data = cloudInfo_cloud_surface_data;


...
}

5、移植过程函数

在移植的中间过程中，不可避免地会出现需要将ros类型转换为自定义类型的情况。

所以为了方便移植过程的进行，所以写了一些ros转自定义类型的函数（最终移植完成就用不到了）

pub_ros2struct.h

/**
 * @brief 
 * 函数名称：pub_change_消息名称
 * 参数：ros消息类型，struct消息类型
 * 
 * 调用的时候传引用
 */
void pub_change_lio_sam_msgs_cloud_info(lio_sam::cloud_info &cloudInfo, lio_sam_msgs_cloud_info &cloudInfo_sharemem, int32_t cloudInfo_startRingIndex[], int32_t cloudInfo_endRingIndex[], int32_t cloudInfo_pointColInd[], float cloudInfo_pointRange[], sensor_msgs_PointField cloudInfo_cloud_deskewed_fields[], uint8_t cloudInfo_cloud_deskewed_data[], sensor_msgs_PointField cloudInfo_cloud_corner_fields[], uint8_t cloudInfo_cloud_corner_data[], sensor_msgs_PointField cloudInfo_cloud_surface_fields[], uint8_t cloudInfo_cloud_surface_data[])
{
        cloudInfo_sharemem.header.seq = cloudInfo.header.seq;
        cloudInfo_sharemem.header.stamp.nsec = cloudInfo.header.stamp.nsec;
        cloudInfo_sharemem.header.stamp.sec = cloudInfo.header.stamp.sec;
        strcpy(cloudInfo_sharemem.header.frame_id, cloudInfo.header.frame_id.c_str());
        // 这里的指针就不赋值了
        // cloudInfo_sharemem.startRingIndex = cloudInfo.startRingIndex;
        // 怎么判断ros数组的大小 明天写个demo试一下子

        cloudInfo_sharemem.startRingIndex_number = cloudInfo.startRingIndex.size();
        cloudInfo_sharemem.endRingIndex_number = cloudInfo.endRingIndex.size();
        cloudInfo_sharemem.pointColInd_number = cloudInfo.pointColInd.size();
        cloudInfo_sharemem.pointRange_number = cloudInfo.pointRange.size();

        cloudInfo_sharemem.imuAvailable = cloudInfo.imuAvailable;
        cloudInfo_sharemem.odomAvailable = cloudInfo.odomAvailable;

        cloudInfo_sharemem.imuRollInit = cloudInfo.imuRollInit;
        cloudInfo_sharemem.imuPitchInit = cloudInfo.imuPitchInit;
        cloudInfo_sharemem.imuYawInit = cloudInfo.imuYawInit;

        cloudInfo_sharemem.initialGuessX = cloudInfo.initialGuessX;
        cloudInfo_sharemem.initialGuessY = cloudInfo.initialGuessY;
        cloudInfo_sharemem.initialGuessZ = cloudInfo.initialGuessZ;
        cloudInfo_sharemem.initialGuessRoll = cloudInfo.initialGuessRoll;
        cloudInfo_sharemem.initialGuessPitch = cloudInfo.initialGuessPitch;
        cloudInfo_sharemem.initialGuessYaw = cloudInfo.initialGuessYaw;

        // 楼下这几个不能直接赋值
        // cloudInfo_sharemem.cloud_deskewed = cloudInfo.cloud_deskewed;
        // cloudInfo_sharemem.cloud_corner = cloudInfo.cloud_corner;
        // cloudInfo_sharemem.cloud_surface = cloudInfo.cloud_surface;

        sensor_msgs_PointCloud2 cloud_deskewed,cloud_corner,cloud_surface;

        cloud_deskewed.header.seq = cloudInfo.cloud_deskewed.header.seq;
        cloud_deskewed.header.stamp.nsec = cloudInfo.cloud_deskewed.header.stamp.nsec;
        cloud_deskewed.header.stamp.sec = cloudInfo.cloud_deskewed.header.stamp.sec;
        strcpy(cloud_deskewed.header.frame_id, cloudInfo.cloud_deskewed.header.frame_id.c_str());
        cloud_deskewed.height = cloudInfo.cloud_deskewed.height;
        cloud_deskewed.width = cloudInfo.cloud_deskewed.width;
        cloud_deskewed.fields_number = cloudInfo.cloud_deskewed.fields.size();
        cloud_deskewed.is_bigendian = cloudInfo.cloud_deskewed.is_bigendian;
        cloud_deskewed.point_step = cloudInfo.cloud_deskewed.point_step;
        cloud_deskewed.row_step = cloudInfo.cloud_deskewed.row_step;
        cloud_deskewed.data_number = cloudInfo.cloud_deskewed.data.size();
        cloud_deskewed.is_dense = cloudInfo.cloud_deskewed.is_dense;
.......
}

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

在项目中用到了很多ros原生的函数，并且在项目的utility.h文件中，自定义了许多与ros有关的函数。

针对这些函数，我的想法是：在遇到的时候将函数进行重写，重现成我们项目可以直接使用的函数。

分类：

• ros与其他库结合的函数（如pcl等）
• utility.h

在本周的移植工作中，重写的函数：

pcl/pcl.h

namespace pcl_struct
{
    double toSec(msgs_Time time)
    {
        return (static_cast<double>(time.sec) + 1e-9*static_cast<double>(time.nsec));
    }
    uint64_t toNSec(msgs_Time time)
    {
        return static_cast<uint64_t>(time.sec)*1000000000ull + static_cast<uint64_t>(time.nsec); 
    }
}

    void copyPointCloud2MetaData(const sensor_msgs_PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
    {

        toPCL(pc2.header, pcl_pc2.header);

        pcl_pc2.height = pc2.height;
        pcl_pc2.width = pc2.width;
        toPCL(pc2.fields, pc2.fields_number, pcl_pc2.fields);

        pcl_pc2.is_bigendian = pc2.is_bigendian;

        pcl_pc2.point_step = pc2.point_step;

        pcl_pc2.row_step = pc2.row_step;

        pcl_pc2.is_dense = pc2.is_dense;

    }

...省略一下中间函数
void toPCL(const sensor_msgs_PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
{

    copyPointCloud2MetaData(pc2, pcl_pc2);

    // pcl_pc2.data = pc2.data;
    //是不是没有resize
    pcl_pc2.data.resize(pc2.data_number);
    for(int i = 0; i <(int) pc2.data_number; i++) {
        pcl_pc2.data[i] = pc2.data[i];
    }
}
template <typename T>
void fromROSMsg(const sensor_msgs_PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud )
{
    pcl::PCLPointCloud2 pcl_pc2;
    //  pcl_conversions::toPCL(cloud, pcl_pc2);
    toPCL(cloud, pcl_pc2);
//  std::cout<<"sss"<<endl;
    pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
// std::cout<<"aaa"<<endl;
}

技术难点

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

ros原生函数的重写

bug记录

bug1：Failed to find match for fields ‘x/y/z/’

网上搜的，该问题一般都是传入的pcl点云数据类型（eg：piontXYZ）中的 1）传入的数据的精度错误（应该是单精度浮点数，传入的是双精度） 2）传入的数据是空的…

后来发现，是我的传入的pointFiled中的name有点问题（x y z…），然后进一步发现

• sub函数中接收数据时，没有将顶层数据和指针指向的数据连接起来

  

bug2：对vector赋值

1）vector = vector时，直接赋值就行

2）对vector逐一赋值时，需要先resize一下vector的大小

bug3：单精度和双精度的bug

上面的两个bug解决了以后，发现进程已经不会异常退出，但是rviz仍然无法正常显示画面

分析消息传递方式修改前后的rqt_graph图

发现mapOptmization发布给rviz的话题中少了一个

进一步发现一直没有发布那个话题

然后发现时间戳有问题，时间戳一直没变。之后想到是因为时间戳的精度太低。

然后意识到之前的测试 ROS_INFO("%f",)时的问题，那个toSec得到的结果就是双精度double类型，是因为我打印的问题，导致认为结果时单精度的。

修改toSec函数后，整个项目终于能正常跑起来了。

问题4：ros工作空间的问题

两个工作空间的两个项目的名称相同

• echo $ROS_PACKAGE_PATH打印后，发现目前活跃的工作空间的只有catkin_ws空间，此时是能正常工作该工作空间中的项目
• 现在不知道的是因为我修改的项目有问题，还是两个同名会冲突导致问题

现在把修改的项目的名称修改

• 所以是项目的问题，创建文件失败了

先不管创建文件失败的原因，感觉两个项目还是会有冲突

• 把改动的那个项目移动出去了
• 直接修改原始的文件
  • 原始的项目也备份

其他

参考资料：

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

rosTime http://docs.ros.org/en/latest/api/rostime/html/classros_1_1TimeBase.html