相机雷达时间同步（基于ROS）

运行环境：

ubuntu20.04 noetic
usb_cam
速腾Robosense 16线
宏基暗影骑士笔记本

思路：

软同步：订阅相机和雷达原始数据，然后进行时间同步，最后将同步后的数据发布出去，通过rosbag record进行录制

同步前和同步后效果对比

同步前的话题：
/rslidar_packets
/usb_cam/image_raw

 # 录制命令rosbag record -O lidar_camera /usb_cam/image_raw /rslidar_points

同步后的话题:
/sync/img
/sync/lidar

 # 录制命令rosbag record -O lidar_camera_syn_time /sync/img /sync/lidar

1.1创建工作空间

mkdir -p sys_time_ws/src
cd sys_time_ws
catkin_make
code .

1.2创建功能包

1） 在工作空间src目录下创建功能包

sys_time
roscpp rospy std_msgs

2.1编写源文件

1）在 src 目录下新建文件 sub_and_pub.cpp

代码解释：
this 关键字指向类创建的对象
registerCallback 绑定回调函数，触发回调函数发布同步后的数据

#include "sys_time/sub_and_pub.h"# 重写头文件中的构造函数subscriberANDpublisher()
# main函数初始化对象(subscriberANDpublisher sp）时自动调用构造函数
subscriberANDpublisher::subscriberANDpublisher()
{//订阅话题lidar_sub.subscribe(nh, "/rslidar_points", 1);camera_sub.subscribe(nh, "/usb_cam/image_raw", 1);//消息过滤器，使用 ApproximateTime 进行时间同步（允许一定程度的时间误差）sync_.reset(new message_filters::Synchronizer<syncpolicy>(syncpolicy(10), camera_sub, lidar_sub));sync_->registerCallback(boost::bind(&subscriberANDpublisher::callback, this, _1, _2));//发布者camera_pub = nh.advertise<sensor_msgs::Image>("sync/img", 10);lidar_pub = nh.advertise<sensor_msgs::PointCloud2>("sync/lidar", 10);
}void subscriberANDpublisher::callback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::PointCloud2ConstPtr& pointcloud) {ROS_INFO("Received synchronized message!");camera_pub.publish(image);lidar_pub.publish(pointcloud);
}

2.2编写头文件

1）在功能包下的 include/功能包名 目录下新建头文件 sub_and_pub.h
2）配置 includepath 详情见

#ifndef SUB_AND_PUB_H
#define SUB_AND_PUB_H
//ros头文件
#include <ros/ros.h>
//时间同步
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
//传感器消息
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>class subscriberANDpublisher{
public:subscriberANDpublisher();void callback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::PointCloud2ConstPtr &pointcloud);private:ros::NodeHandle nh;ros::Publisher camera_pub;ros::Publisher lidar_pub;message_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub;//雷达订阅message_filters::Subscriber<sensor_msgs::Image> camera_sub;//相机订阅typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> syncpolicy;//时间戳对齐规则typedef message_filters::Synchronizer<syncpolicy> Sync;boost::shared_ptr<Sync> sync_;//时间同步器};
#endif

2.3编写可执行文件

#include <ros/ros.h>
#include "sys_time/sub_and_pub.h"int main(int argc, char **argv) {ros::init(argc, argv, "node");subscriberANDpublisher sp;ROS_INFO("main done! ");ros::spin();
}

2.4配置文件

1）修改CMakeLists.txt

# 添加message_filters ROS包依赖
find_package(catkin REQUIRED COMPONENTSroscpprospystd_msgsmessage_filters
)# 头文件路径
include_directories(include${catkin_INCLUDE_DIRS}
)# 新建c++库，将头文件和源文件添加到新库里面
add_library(sys_time_libinclude/sys_time/sub_and_pub.hsrc/sub_and_pub.cpp
)

# 将src目录下的main.cpp编译成可执行文件
add_executable(main.cpp src/main.cpp)# 将新库和ros库链接起来
target_link_libraries(sys_time_lib${catkin_LIBRARIES}
)# 将可执行文件和新库链接起来
target_link_libraries(main.cppsys_time_lib${catkin_LIBRARIES}
)

2）修改package.xml （实际上不修改也可以通过编译）

  <exec_depend>message_filters</exec_depend>

3.1编译运行

1）运行时间同步节点

# 编译
ctrl+shift+broscore
source ./devel/setup.bash
rosrun sys_time main.cpp

2）启动相机

cd usb_cam_ws
source ./devel/setup.bash
roslaunch usb_cam usb_cam-test.launch

3）启动雷达

cd robosense_ws
source ./devel/setup.bash
roslaunch rslidar_sdk start.launch

当相机雷达全部启动后，显示接受到时间同步消息

4.1录制时间同步后的rosbag

lidar_camera_syn_time 是保存的rosbag名称
/sync/img 和 /sync/lidar 是录制的话题名

rosbag record -O lidar_camera_syn_time /sync/img /sync/lidar

# 查看录制好的rosbag
rosbag info lidar_camera_syn_time.bag

4.2rviz可视化rosbag

配置文件详细步骤见：Robosense激光雷达录制rosbag

1）打开rviz

rviz

2）导入配置文件 file–open cofig

3)添加相机话题

4）播放rosbag

rosbag play lidar_camera_syn_time.bag

⭐⭐⭐嘟嘟崽 ⭐⭐⭐ ⭐⭐⭐祝你成功 ⭐⭐⭐