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

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

日期：4.04 – 4.10

项目进度

本周工作进展：

• 更换了底层的通信框架（shm）
• 熟悉速腾雷达驱动，以及ros相关驱动，为二次开发驱动做准备

1、更换底层通信框架

上周解决的utility读写问题，本周运行之后又出现了之前的错误：进程在初始化后会随机崩溃掉，然后重启，有时候会稳定，有时候会移植崩溃。

暂时查不出原因来，于是回到程序本身，在多线程std::thread、mmap的通信框架里找问题。

然后让师兄看了一下代码，师兄发现了代码的一个大问题。

我们本来用的mmap框架搭建通信机制。

经过师兄的调拨，认识到mmap作为通信框架会出现问题（虽然暂时没有遇到）

先简单梳理一下mmap的工作机制

• 首先，将文件从原本的存储介质中映射到内存的一块区域
• 对于文件的读取可以直接从内存中进行读取，所以mmap的通常用途是用于加快文件读取速率
• 但是多个文件映射同一个文件时，并不像之前我们认为的那样映射到同一块内存区域中，而是各自有各自的内存映射区域。至于为什么每个进程返回的共享内存首地址是同一个值，是因为返回的是虚拟地址，一个相同的虚拟地址可以对应不同的物理地址，当不同的进程通过虚拟地址进行写入时，表面上是对同一个地址进行写入，实际上内核会找到对应的物理地址进行写入
• 而且，写入不会直接写入到内存中，而是会首先写入到缓存中，然后经由缓存写入到磁盘，同时内存的相应位置设置为脏页，其他文件读取时，会从磁盘中将更新的页换入到内存中进行读取。
• 这就造成了使用mmap进行内存共享时，读写时不同步的，时间差取决于系统的负载情况，所以有可能迁移到实际情况中后，效果会明显变差，这取决于底层存储介质的读写速度。

总结一下：mmap机制，共享通过内存–缓存–硬盘–内存–进程

于是我们考虑换掉mmap，使用shm重写一下接口（至于为什么不用socket通信，我只能说那样要改的太多了，只能说开始取巧偷懒了，现在就要填坑了）。

方便的是，由于我们的读写更新速度比较快，所以使用死循环的方式应该会优于使用信号量的方式，所以我们只需要在最底层的通信接口内部，用shm替代mmap

以下是新的通信框架

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


key_t get_id(const string fileName)
{
    key_t ret = 0;
    for(int i = 0; i < fileName.size(); i++)
    {
        ret += fileName[i];
    }
     //cout<
    return ret + 1000000;
}

/**
 * @brief Create a shm object
 * 
 * @param shm_key 共享内存键值
 * @param shm_num 共享内存字节长度
 * @param shm_flag 权限 （默认权限为读写权限）
 * @return void* 
 */
MemoryDefinition* createShareMemory(const string shm_key, size_t shm_num)//, int shm_flag = 1)
{
    MemoryDefinition* shm_point = new MemoryDefinition();;
    int shm_flag = IPC_CREAT | 0666;
    int fd = shmget(get_id(shm_key), shm_num, shm_flag);
    void* point;
    if(fd != -1) {
        //
        point = shmat(fd, 0, 0);
        if((int*)point != (int*)-1) {
            //初始化
            memset(point, 0, shm_num);


            shm_point->fd = fd;
            shm_point->point = point;
            return shm_point;
        } else {
            perror("get shareMemory error");
        }
    } else {
        perror("get shareMemory error");

    }
    return shm_point;
}

/**
 * @brief 连接共享内存
 * 
 * @param shm_key 共享内存键值
 * @param shm_num 共享内存字节长度
 * @param shm_flag 权限 （默认权限为读写权限）
 * @return void* 
 */
MemoryDefinition* connectShareMemory(const string shm_key, size_t shm_num)//, int shm_flag = 1)
{
    MemoryDefinition* shm_point = new MemoryDefinition();
    int shm_flag = IPC_CREAT | 0666;
    int fd = shmget(get_id(shm_key), shm_num, shm_flag);
    void* point;
    if(fd != -1) {
        //
        point = shmat(fd, 0, 0);
        if((int*)point != (int*)-1) {
            shm_point->fd = fd;
            shm_point->point = point;
            cout<<shm_key<<' '<<fd<<' '<<point<<endl;
            return shm_point;
        } else {
            perror("get shareMemory error");
        }
    } else {
        perror("get shareMemory error");
    }
    return shm_point;
}

void unMap(const MemoryDefinition* definition)
{
    shmdt(definition->point);

}

#endif

修改后项目能够正常进行

预想的超过共享内存上限值的问题没有发生（查查多少MB）

而且改成shm后，今天最开始要解决的进程崩溃问题好了。说明不是读取文件流的问题，是mmap本身的问题

2、速腾雷达驱动 – 文档阅读

最开始，我的计划是针对雷达的通信方式，直接从0开始写一个驱动程序。于是开始阅读速腾雷达的官方文档。

网址： https://www.robosense.cn/resources-27

下面是一些记录：

UDP通信协议

数据包存在于MSOP包中

发送间隔1.33ms

单回波和双回波

https://www.zhihu.com/question/473355887/answer/2011499085

• 激光发射后 随着距离的增大，会出现散射的现象；当激光到达透明物体或者物体边缘时，会有部分散射的激光继续前进

• 双回波会收集两次回波，但是可能会造成数据的重复收集

• 只反射了一次回波。但是接收器仍然将其识别为两次回波。

数据包格式分析

帧头 42byte

1 - 8byte：用于包头检测

• 序列：0x55,0xAA,0x05,0x0A,0x5A,0xA5,0x50,0xA0

21 - 30byte：存储时间戳

• 

• 
  
  

31byte：表示激光雷达的型号

数据段

数据块区间是 MSOP 包中传感器的测量值部分，共 1200byte。它由 12 个 data block 组成，每个 block 长度为 100byte，代表一组完整的测距数据。Data block 中 100byte 的空间包括：2byte 的标志位，使用 0xffee 表示；2byte 的 Azimuth，表示水平旋转角度信息，每个角度信息对应着 32 个的 channel data，包含 2 组完整的 16 通道信息。

3、rslidar 驱动 二次开发 准备

看了半天，发现从头写不太现实，于是还是去网上看一下ros环境下的使用流程。

整理出一些有用的文章：

ROS功能包在线把速腾聚创点云格式转为velodyne点云格式 https://blog.csdn.net/weixin_53073284/article/details/123344981

libpcap详解 https://blog.csdn.net/superbfly/article/details/51199161 (数据包捕获函数库，是Unix/Linux平台下的网络数据包捕获函数库。它是一个独立于系统的用户层包捕获的API接口，为底层网络监测提供了一个可移植的框架。)

https://github.com/RoboSense-LiDAR/rslidar_sdk Rlidar_SDK是运行在Ubuntu操作系统上的激光雷达驱动软件开发工具包，它包含了激光雷达驱动核、ROS支持、ROS 2支持和Protobuf-UDP通信功能。对于希望通过ROS或ROS 2获得点云的用户，可以直接使用此软件开发工具包。对于那些想要做高级开发或将激光雷达驱动器集成到他们自己的项目中的人，请参考激光雷达驱动器核心rs_Driver。

https://blog.csdn.net/weixin_53073284/article/details/123344981 ROS功能包在线把速腾聚创点云格式转为velodyne点云格式

https://blog.csdn.net/weixin_53073284/article/details/122680587 速腾rs16激光雷达安装驱动使用方法

所以如果在ros的环境下 大致流程是

• 安装速腾雷达的驱动
• 二次开发一下？（可能是点云格式的一些问题）
• （如果直接需要速腾格式的）直接发布就行了
• （如果需要velodyne格式的）经过rs_to_velodyne进行转换（流程就是订阅上面发布的话题，转换成对应格式继续发布）
• 订阅话题（后面要注意一下，是否需要将坐标系继续转换 （取决于算法的实现 需要再看））

所以我现在的流程

• 安装速腾雷达的驱动（不使用里面的ros的相关功能）
• 二次开发（根据ros的实现，转换为我们自定义的点云格式）
• 根据rs_to_velodyne的实现，继续转换点云格式
• 自定义发布

所以现在不需要我从头开始写UDP编程了，接着速腾雷达的驱动进行二次开发

• 安装驱动

看到了一个同学的博客

https://blog.csdn.net/weixin_53073284/article/details/122680587

现根据他的博客，安装了一个驱动，这个驱动应该是专门为ros设计的

git clone https://github.com/BluewhaleRobot/ros_rslidar_robosense.git

这个在我编译的时候出了点小问题

• 一直提示我一个头文件下的类没有定义
• 经过查找是链接的时候出的问题

解决：

• 先找到pcl库的路径
  • 小技巧：在编译的时候打印依赖的库的信息
  • 找到了对应的文件
  • 然后发现缺失的那个头文件 不在打印的信息里面
  • 所以在链接的地方添加这个路径即可

#add_dependencies(talker ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(range_node 
	range_conversion
	${catkin_LIBRARIES}
  /usr/lib/x86_64-linux-gnu/libpcl_visualization.so
)

#message(STATUS "?????????????????????????????????????????????????????????????????? ${catkin_LIBRARIES}")

核心驱动包 :

两个主要的节点

• rslidar_driver
• rslidar_pointcloud

针对代码的具体分析体现在代码中，不在博客中记录了。

这里只记录一下一些概念和主要流程：

msgs格式

rslidarScan.msg

# LIDAR scan packets.

Header           header         # standard ROS message header
rslidarPacket[] packets        # vector of raw packets

rslidarPacket.msg

# Raw LIDAR packet.

time stamp              # packet timestamp
uint8[1248] data        # packet contents

1248字节是一个UDP协议包的有效荷载字长

Scan则是一个packet的数组，保存一组packet

基于发布的话题

  msop_output_ = node.advertise("rslidar_packets", 10);

可以认为，订阅了该话题的节点 是用于处理数据的

关于水平转角的一个信息

• 差值

RS-LiDAR-16 每隔一组 16 线激光测距才输出一次水平旋转角度信息，因此在单回 波模式下，对于没有输出水平旋转角度信息的那组 16 线激光测距需要通过插值来获得。 有很多种方式可以插值，下面的方法是最简单和直接的一种。 对于一个 Packet 中的数据，Block 1 和 Block 2 的第一个数据采集的时间间隔是 ~100us，可以认为在这个期间雷达是匀速旋转的。因此可以计算第 N+1 组 16 线激光测 距的第一个数据的水平角度是第 N 组 16 线激光测距的第一个数据的水平角度和第 N+2 组 16 线激光测距的第一个数据的水平角度的均值

rawdata.h

最上面定义了一些静态的雷达相关参数（暂时没看）

struct raw_block

// block
typedef struct raw_block //一个block 2byte 的标志位，使用 0xffee 表示；2byte 的 Azimuth，表示水平旋转角度信息，每 个角度信息对应着 32 个的 channel data，包含 2 组完整的 16 通道信息。
{
  uint16_t header;  ///< UPPER_BANK or LOWER_BANK
  uint8_t rotation_1; 
  uint8_t rotation_2;  /// combine rotation1 and rotation2 together to get 0-35999, divide by 100 to get degrees
  uint8_t data[BLOCK_DATA_SIZE];  // 96 32 * 2(一个block出去标志位和水平转角信息的数据大小)
} raw_block_t;

struct raw_packet

整个数据包

• revolution和status参数还没搞懂是干嘛的

loadConfigFile函数

从配置文件中加载参数

• 原本的项目中没有对应的配置文件
• 我从其他包中找到了对应的参数
• 但是可能还要根据雷达具体的参数来判断（比如角度等等）

在项目接受的点云数据中：基本上每帧的width都是 16000左右

与楼下这个数字基本上接近

• 代表的是有多少个点
  • 16：16线
  • 24：一个包扫描了24次（对应于不同的水平角度）
  • 84：进行了84次扫描
  • /2不知道为啥

unpack函数

从第43byte开始解析（前42byte是head部分）

然后逐个block解析（12个）

更新温度信息（不重要）

计算水平偏角

• 配合深度信息计算xyz

• 一组通道中的每个通道的水平偏角，按照源代码的计算方式来看，是不同的

• int azi1, azi2; // 插值计算一个block中的后半段的角度
        azi1 = 256 * raw->blocks[block + 1].rotation_1 + raw->blocks[block + 1].rotation_2;
        azi2 = 256 * raw->blocks[block].rotation_1 + raw->blocks[block].rotation_2;
        azimuth_diff = (float)((36000 + azi1 - azi2) % 36000);
  
  
  for (int firing = 0, k = 0; firing < RS16_FIRINGS_PER_BLOCK; firing++)  // 2
      {
        for (int dsr = 0; dsr < RS16_SCANS_PER_FIRING; dsr++, k += RAW_SCAN_SIZE)  // 16   3
        {
          //计算出每个通道的水平偏角
          azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr * RS16_DSR_TOFFSET) + (firing * RS16_FIRING_TOFFSET)) /
                                           RS16_BLOCK_TDURATION); //
          //取整
          azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000;  // convert to integral value...
  
  

• 计算方法：

  • azimuth_diff代表的是第i个block的第一个通道的偏角 与 第i+1个block的第一个通道的偏角的差值（最后一个block用的与前一个block的差值）
  • 后面计算每个通道的水平偏角
    • RS16_DSR_TOFFSET 代表每个通道扫描的时间间隔
    • RS16_FIRING_TOFFSET 代表第一个组16通道扫描的时间
    • RS16_BLOCK_TDURATION 代表一组block扫描的时间（RS16_FIRING_TOFFSET 的二倍）
    • 通过上面的公式计算出每个通道的水平偏角

然后直接将intensity信息赋值

注：

这里发布的点云信息不具备ring信息和stamp信息

• 这里结合rs_to_velodyne的代码
• time可以直接用ros的时间戳
  • 如果不行的，手动赋值
  • 通过起始的ros时间戳+差值的方式（均匀
  • 看了lio的项目，雀氏是需要每个点的时间戳的
  • 雀氏可以手动赋值
  • 先看一下原始代码吧
• ring直接赋值通道数就可以了应该（而且应用的时候貌似也不需要ring？）

认为：

• rs雷达的time格式是时间点
• velodyne雷达的time格式是该点的时间戳 与 这一组点云的开始点的时间戳的 差值

而项目里实际需要的就是时间戳

• 在项目中应该有转换成实际时间戳的操作

优化方向

• 接收的点云数据 加上角度信息

• rslidar_node.cpp中启动该节点，首先初始化一个rslidar_driver::rslidarDriver类

  • 构造函数中，从配置文件中读取一系列参数，并且初始化话题的发布（msop和difop，我们需要的是msop）
    • 发布的话题的消息类型是：rslidar_msgs::rslidarScan，在后面解释
  • 并且初始化socket通信，在input.cc中，通过socket网络编程与雷达建立起通信连接。

• 节点后面while循环执行楼上类的poll函数，该函数用于轮询雷达设备

  • 初始化rslidarPacket，读取npackets次数据包（84），通过input中的getpacket函数

• rslidar_pointcloud中的节点订阅rslidar_driver发布的话题

• 将原始数据解析成点云数据

• 发布

4、rs_to_velodyne

还需要注意的是，大多数slam项目所适配的点云格式都是velodyne雷达（一个国外的牌子），而我们项目所使用的是国内的速腾雷达，产生的点云格式与velodyne有些许差别，需要转换。github上找到了相关程序，不过是针对ros的，需要改动。

https://github.com/HViktorTsoi/rs_to_velodyne

大体思路就是：

• 订阅发布的rslidar点云话题
• 转换
• 发布velodyne格式点云话题

梳理完成，可以开始进行驱动的编写了

技术难点

rdlidar雷达驱动

socket通信

bug记录

没啥大bug

其他

无