相机雷达时间同步（基于ROS）_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

# 查看bag时间戳
rqt_bag lidar_camera.bag
1
2
3
4
5

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

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

1.1创建工作空间

mkdir -p sys_time_ws/src
cd sys_time_ws
catkin_make
code .
1
2
3
4

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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

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();
}
1
2
3
4
5
6
7
8
9
10

2.4配置文件

1）修改CMakeLists.txt

# 添加message_filters ROS包依赖
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_filters
)

# 头文件路径
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

# 注意：根据include文件夹位置去修改
# 新建c++库，将头文件和源文件添加到新库里面
add_library(sys_time_lib
  include /sys_time/sub_and_pub.h
  src/sub_and_pub.cpp
)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

# 将src目录下的main.cpp编译成可执行文件
add_executable(main.cpp src/main.cpp)

# 将新库和ros库链接起来
target_link_libraries(sys_time_lib
  ${catkin_LIBRARIES}
)

# 将可执行文件和新库链接起来
target_link_libraries(main.cpp
  sys_time_lib
  ${catkin_LIBRARIES}
)
1
2
3
4
5
6
7
8
9
10
11
12
13

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

  <exec_depend>message_filters</exec_depend>
1

3.1编译运行

1）运行时间同步节点

# 编译
ctrl+shift+b

roscore
source ./devel/setup.bash
rosrun sys_time main.cpp
1
2
3
4
5
6

2）启动相机

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

3）启动雷达

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

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

4.1录制时间同步后的rosbag

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

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

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

4.2rviz可视化rosbag

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

1）打开rviz

rviz
1

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

3)添加相机话题

4）播放rosbag

rosbag play lidar_camera_syn_time.bag
1

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