RoboSense R128激光雷达+海康威视全景摄像头ROS采集数据及MATLAB联合标定

前言

老师那边突然说要做激光雷达联合相机采集数据的实验，时间紧任务重，在这个过程中也不可避免遇到了许多的坑，遂记录下来，同时也作为一个教程给大家

顺便写一下我的配置：

Ubuntu 20.04 + ROS1 noetic

速腾雷达 RS-Ruby 128线

海康威视全景相机 DS-2CD6984F

MATLAB2024a

程序如下：https://github.com/li01233/mul_modal_ROS.git

一  准备工作

1 雷达连接

RoboSense激光雷达使用(Windows)_激光雷达和电脑通讯-CSDN博客

这部分其实速腾官网的说明文档和网上的一些文档说得很清楚了，只要把自己电脑设成固定网段192.168.1.xxx就行了，但我各种配，就是连上了没数据

中间各种折腾，又是关防火墙又是设置入站规则的，还禁用了所有的网卡

最后发现问题相当逆天……

首先，用Wireshark可以确定雷达是192.168.1.200，然后打开cmd是ping得通的

接下来，打开浏览器输入192.168.1.200，进入雷达初始配置界面：

注意到，第一是Destination IP Address这里，要设成你的电脑的固定IP：192.168.1.xxx

第二，就是Rotation Speed不能为0……（不然完全没有数据出来）（为什么你不转都能升到70度啊！）

至于是如何发现我压根没开雷达……在Wireshark中我只看到了来自7788端口的数据（DIFOP一般是设备的一些信息），而没有6699的（MSOP是点云的），所以很明显……但我是万万没想到他没转啊！

2 相机连接

相机就简单太多了，首先用SADP找到相机的IP（默认一般是192.168.1.64，http端口80），并且在这个软件里激活相机

海康威视摄像头初始化设置（新相机的第一次配置&相机恢复出厂设置）_海康威视摄像头初始化软件-CSDN博客

最后在浏览器里输入192.168.1.64：http端口，然后愉快输入用户名admin，密码（你刚才激活时设置的），就可以看到画面了

3 固定支架制作

这部分就不写了，反正你既然要联合标定，肯定要把这俩的相对位置固定了

这里我直接solidworks建模了一个，找工厂做了

二  ROS安装与驱动编译

1 ROS安装

ROS安装属于老生常谈的话题，我推荐看这一篇，因为我看其他的多多少少碰过问题

1.2.4 安装 ROS · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程

遇到连接不到packages.ros.org问题可以看我另一篇博客

ROS安装时连接不到packages.ros.org解决方法-CSDN博客

2 雷达驱动下载与编译

激光雷达是官方有sdk，所以直接到他底下下载了，然后根据readme一步一步做的

首先下源码

git clone https://github.com/RoboSense-LiDAR/rslidar_sdk.git
cd rslidar_sdk
git submodule init
git submodule update

安装依赖库

sudo apt-get update
sudo apt-get install -y libyaml-cpp-dev
sudo apt-get install -y  libpcap-dev

编译运行：

(1) 新建一个文件夹作为工作空间，然后再新建一个名为src的文件夹, 将rslidar_sdk工程放入src文件夹内。

(2) 返回工作空间目录，执行以下命令即可编译&运行(若使用.zsh,将第二句指令替换为 source devel/setup.zsh)。

catkin_make
# 用了conda环境的用这一行
# catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3
source devel/setup.bash
roslaunch rslidar_sdk start.launch

然后就会在你的rviz中愉快地显示点云了～

3 相机驱动下载与编译

这部分我一开始没看清我相机的型号，直接参考了GitHub - luckyluckydadada/HIKROBOT-MVS-CAMERA-ROS: The ros driver package of Hikvision Industrial Camera SDK.的方法，又是安装海康的MVS软件又是折腾BLAS什么什么的库，在那把BLAS，CBLAS，LAPACKE什么全部装好了，才发现根本搜不到我的相机……MVS软件里也找不到，一搜，只支持工业相机啊！

好吧，从头再来

还好让我找到了我这种DS打头的监控相机的ROS驱动，感谢感谢！

GitHub - tanzby/hikvision_ros: simple preview bridge node of hikvision IP camera for ROS

同样的把它放在工作空间目录的src目录下，和rslidar_sdk同一层，然后catkin_make即可

git clone https://github.com/tanzby/hikvision_ros.git
# cd到工作空间根目录
catkin_make

三  数据录制

1 改配置文件

检查相机配置：在src/hikvision/launch/hik.launch中更改ip等如下

<arg name="ip_addr" default="192.168.1.xxx"/>
<arg name="user_name" default="admin"/>
<arg name="password" default="********"/>

因为我的全景相机有多个通道，所以我复制粘贴了多个节点，每个节点手动设置了通道，在src/hikvision/launch/hik.launch如下

<node pkg="hikvision_ros" type="hik_cam_node" name="hik_cam_node_1" output="screen">

    <!-- modify LD_LIBRARY_PATH for loading hikvision sdk -->
    <env name="LD_LIBRARY_PATH"
         value="$(env LD_LIBRARY_PATH):$(find hikvision_ros)/lib:$(find hikvision_ros)/lib/HCNetSDKCom"/>

    <param name="ip_addr" value="$(arg ip_addr)" />
    <param name="user_name" value="$(arg user_name)" />
    <param name="password" value="$(arg password)" />
    <param name="port" value="$(arg port)" />
    <param name="channel" value="1"/>
    <param name="link_mode" value="$(arg link_mode)"/>

    <param name="image_width" value="$(arg image_width)"/>
    <param name="image_height" value="$(arg image_height)"/>
    <param name="camera_frame_id" value="$(arg camera_frame_id)"/>
    <param name="camera_name" value="$(arg camera_name)"/>
    <param name="camera_info_url" value="$(arg camera_info_url)"/>
</node>

检查雷达配置：在src/rslidar_sdk/config/config.yaml中更改雷达型号为RS128

2 启动！

运行ros

roscore

启动新的一个终端，用于运行相机节点

source devel/setup.sh
roslaunch hikvision_ros hik.launch

再启动一个终端，用于运行雷达节点

source devel/setup.bash
roslaunch rslidar_sdk start.launch

在弹出的rviz中可以添加image，用于同时展示图像和点云数据。使用rostopic list可以查看发布了的话题

3 录制并解析数据

启动一个新终端，使用rosbag记录数据xxx.bag（这里我录制的是压缩了的图像数据，不然太大了）

rosbag record /rslidar_points /hik_cam_node_1/hik_camera/compressed /hik_cam_node_2/hik_camera/compressed /hik_cam_node_3/hik_camera/compressed /hik_cam_node_4/hik_camera/compressed -O xxx

解析bag数据到jpg和pcd，用时间戳命名，这里参考了从bag包中提取图片和点云数据为pcd格式点云文件-CSDN博客

#! /usr/bin/python
# coding=utf-8
 
import os
import rosbag
import cv2
from cv_bridge import CvBridge
from tqdm import tqdm
 
class ExtractBagData(object):
 
    def __init__(self, bagfile_path, camera_topic, pointcloud_topic, root):
        self.bagfile_path = bagfile_path
        self.camera_topic = camera_topic
        self.pointcloud_topic = pointcloud_topic
        self.root = root
        self.image_dir = os.path.join(root, "images")
        self.pointcloud_dir = os.path.join(root, "pointcloud")
 
        #创建提取图片和点云的目录 ./root/images  root/pointcloud
        if not os.path.exists(self.image_dir):
            os.makedirs(self.image_dir)
        if not os.path.exists(self.pointcloud_dir):
            os.makedirs(self.pointcloud_dir)
 
    def extract_camera_topic(self):
        bag = rosbag.Bag(self.bagfile_path, "r")
        bridge = CvBridge()
        for i in range(1,len(self.camera_topic)+1):
            bag_data_imgs = bag.read_messages(self.camera_topic[i-1])
            image_dir = os.path.join(self.image_dir, str(i))
 
            index = 0
    
            pbar = tqdm(bag_data_imgs)
            for topic, msg, t in pbar: # type: ignore
                pbar.set_description("Processing extract image id: %s" % (index+1))
                cv_image = bridge.compressed_imgmsg_to_cv2(msg, "bgr8")
                # 如果你需要使用时间戳对提取的图片命名，可以使用msg.header.stamp.to_sec()获取时间戳
                timestr = "%.6f" %  msg.header.stamp.to_sec()
                cv2.imwrite(os.path.join(image_dir, str(timestr) + ".jpg"), cv_image) # type: ignore
                index += 1
 
    def extract_pointcloud_topic(self):
        '''
        # 提取点云数据为pcd后缀文件,默认提取以为时间戳命名
        # 提取命令:rosrun pcl_ros bag_to_pcd result.bag /velodyne_points ./pointcloud
        # 提取点云以时间戳命令:1616554905.476288682.pcd
        :return:
        '''
        cmd = "rosrun pcl_ros bag_to_pcd %s /rslidar_points %s" % (self.bagfile_path, self.pointcloud_dir)
        os.system(cmd)
 
        # 再读取提取的pcd点云数据，把文件名修改为按照顺序索引名
        #pcd_files_list = os.listdir(self.pointcloud_dir)
        # 因为提取的pcd是以时间戳命令的，但是存放到列表中并不是按照时间戳从小到大排列，这里对时间戳进行重新排序
        #pcd_files_list_sorted = sorted(pcd_files_list)
        # print(zip(pcd_files_list, pcd_files_list_sorted))
 
        #index = 0
        #pbar = tqdm(pcd_files_list_sorted)
        #for pcd_file in pbar:
        #    pbar.set_description("Processing extract poindcloud id: %s" % (index + 1))
        #    os.rename(os.path.join(self.pointcloud_dir, pcd_file),
        #              os.path.join(self.pointcloud_dir, str(index) + ".pcd"))
        #    print("pcd_file name: ", pcd_file)
        #    index += 5
 
if __name__ == '__main__':
    #需要处理的bag包的路径
    bagfile_path = './2024-11-30-22-02-42.bag'
    camera_topic = ["/hik_cam_node_1/hik_camera/compressed","/hik_cam_node_2/hik_camera/compressed",
                    "/hik_cam_node_3/hik_camera/compressed","/hik_cam_node_4/hik_camera/compressed"]
    pointcloud_topic = "/rslidar_points"
    #将bag转换为pcd和jpg格式的保存路径
    extract_bag = ExtractBagData(bagfile_path, camera_topic, pointcloud_topic,  "./")
    extract_bag.extract_camera_topic()
    extract_bag.extract_pointcloud_topic()
    

四  联合标定

上面的步骤采集到了pcd和jpg成帧的数据，就可以直接转移到MATLAB中进行联合标定了

激光雷达和相机联合标定保姆级教程_激光雷达标定-CSDN博客

MATLAB要R2022以上的～

这部分就是纯折磨了，没什么了

五 消息同步与消除NaN

消息同步和消除NaN部分我独立建了一个包mess_sync，使用message_filter去同步这么4个相机+1个点云共五个话题，同时使用pcl库，将有序点云转为无序点云，并清除其中的NaN值，大大缩小了文件的大小

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CompressedImage.h>
#include <ros/ros.h>
#include <iostream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;

ros::Publisher Img_info_pub1, Img_info_pub2, Img_info_pub3, Img_info_pub4;
ros::Publisher PC_info_pub;

void callback(const sensor_msgs::PointCloud2::ConstPtr& ori_pointcloud, const sensor_msgs::CompressedImage::ConstPtr& ori_img1,
              const sensor_msgs::CompressedImage::ConstPtr& ori_img2, const sensor_msgs::CompressedImage::ConstPtr& ori_img3,
              const sensor_msgs::CompressedImage::ConstPtr& ori_img4)
{
  // 指针转为msg
  sensor_msgs::PointCloud2 syn_pointcloud = *ori_pointcloud;
  sensor_msgs::CompressedImage syn_img1 = *ori_img1;
  sensor_msgs::CompressedImage syn_img2 = *ori_img2;
  sensor_msgs::CompressedImage syn_img3 = *ori_img3;
  sensor_msgs::CompressedImage syn_img4 = *ori_img4;
  // 打印结果
  cout << "syn velodyne points‘ timestamp : " << syn_pointcloud.header.stamp << endl;
  cout << "syn Img1‘s timestamp : " << syn_img1.header.stamp << endl;
  cout << "syn Img2‘s timestamp : " << syn_img2.header.stamp << endl;
  cout << "syn Img3‘s timestamp : " << syn_img3.header.stamp << endl;
  cout << "syn Img4‘s timestamp : " << syn_img4.header.stamp << endl;

  // 发送话题
  Img_info_pub1.publish(syn_img1);
  Img_info_pub2.publish(syn_img2);
  Img_info_pub3.publish(syn_img3);
  Img_info_pub4.publish(syn_img4);

  pcl::PointCloud<pcl::PointXYZI>* cloud = new pcl::PointCloud<pcl::PointXYZI>;
  pcl::fromROSMsg(*ori_pointcloud, *cloud);

  std::vector<int> mapping;
  pcl::removeNaNFromPointCloud(*cloud, *cloud, mapping);

  sensor_msgs::PointCloud2 out_pointcloud;
  pcl::toROSMsg(*cloud, out_pointcloud);

  PC_info_pub.publish(out_pointcloud);
}


int main(int argc, char** argv)
{
  // 初始化节点
  ros::init(argc,argv,"img");
  ros::init(argc,argv,"pointcloud");
  ros::init(argc, argv, "msg_synchronizer");
  ros::NodeHandle nh;
  // 初始化发布话题
  Img_info_pub1 = nh.advertise<sensor_msgs::CompressedImage>("/compressedimg1",10);
  Img_info_pub2 = nh.advertise<sensor_msgs::CompressedImage>("/compressedimg2",10);
  Img_info_pub3 = nh.advertise<sensor_msgs::CompressedImage>("/compressedimg3",10);
  Img_info_pub4 = nh.advertise<sensor_msgs::CompressedImage>("/compressedimg4",10);
  PC_info_pub = nh.advertise<sensor_msgs::PointCloud2>("/pointcloud",10);
  // 初始化消息订阅者
  message_filters::Subscriber<CompressedImage> camera_sub1(nh, "/hik_cam_node_1/hik_camera/compressed", 1);
  message_filters::Subscriber<CompressedImage> camera_sub2(nh, "/hik_cam_node_2/hik_camera/compressed", 1);
  message_filters::Subscriber<CompressedImage> camera_sub3(nh, "/hik_cam_node_3/hik_camera/compressed", 1);
  message_filters::Subscriber<CompressedImage> camera_sub4(nh, "/hik_cam_node_4/hik_camera/compressed", 1);
  message_filters::Subscriber<PointCloud2> velodyne_sub(nh, "/rslidar_points", 1);
  // 初始化同步消息规则
  typedef sync_policies::ApproximateTime<PointCloud2, CompressedImage, CompressedImage, CompressedImage, CompressedImage> MySyncPolicy;
  // 消息同步与回调   
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), velodyne_sub, camera_sub1, camera_sub2, camera_sub3, camera_sub4); //queue size=10
  sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4, _5));

  ros::spin();
  return 0;
}

六 优化

最后我把所有要启动的节点都写到了一个package里去了，省的开特别多的终端。同时rosbag play我也写到了这个package的另一个launch文件里去了，具体请看README.md