SDK运行与同步
同步板支持串口和网口两种通信方式，实现
- 在SDK中获取当前系统的基准时间并且与同步板进行实时的软件时间对齐
- 实时获取IO脉冲触发的时刻，以及IMU的反馈数据。
其中，
- Type-C的串口同时支持供电和通信，使用简单方法。但是受限于串口通信的波特率，软件同步精度较低，并且目前最高以30Hz频率实时获取IO触发时刻。
- 网口不支持供电，需要用串口供电或者12V电源供电，但是通信速度高、时间同步精度高，最高测试100Hz的频率获取IO触发时刻。
在之前的文档中，考虑到测试的便捷性，我们全都默认使用串口进行同步。但是在硬件支持的情况下，我们强烈建议使用网口作为通信方式，获取最高的精度和运行效率。
注意 ！！！！：一旦同步板在硬件上接了网口，串口的功能将会被禁用，直到断电重启。因此如果同时接入网口和串口，只会使用网口，并且只能使用网口对应的SDK。
当网线插入同步板后,同步板重新上电后会自动使用网口传输信息. 如果同步板IP配置为192.168.1.188.则可以通过ping查看网口配置是否成功或者同步板是否在线.
ping 192.168.1.188
用户可以使用nc指令查看上传的数据，Linux中nc命令是一个功能强大的网络工具，全称是netcatz.
sudo apt-get install netcat #下载打开任意终端，并输入以下指令：
# 使用前修改主机IP为192.168.1.xxx。(同一网段内即可)
# 192.168.1.188 8888 表示同步板的IP地址和端口
echo “Hello InfiniteSense” | nc -u 192.168.1.188 8888 
sudo apt-get install libzmq3-dev #默认版本4.3.4
git clone git@github.com:InfiniteSenseLab/SimpleSensorSync.git -b main
cd SimpleSync
mkdir build && cd build
cmake ..
make -j如果git clone 命令报错，无法下载。可以直接在本项目 页面中的Code/Download ZIP 按键下载。
由于不同的传感器同步组合较多，我们提供一些常用的示例参考。满足如下传感器同步需求的可以直接跳转到对应部分，我们也会不断更新，以提供更多的解决方案示例。
使用示例
- 多个海康/大恒/大华工业相机与同步板的IMU构成VIO系统
- 多个USB免驱相机与同步板的IMU构成VIO系统
- MID360 和 多个海康/大恒/大华工业相机同步
- MID360 和 多个海康/大恒/大华工业相机同步——FAST-LIVO 方案
- MID360 和 多个USB免驱相机 同步——FAST-LIVO 方案
- 速腾雷达和多个工业相机同步
- 速腾雷达和多个USB免驱相机同步
- 外部触发高性能IMU和多个海康/大恒/大华工业相机构成VIO系统
对于在使用示例之外的同步需求，都是能够满足的。这是由于我们的SDK是开源的、支持二次开发的，因此需要用户具有一定的开发能力。如果有需要可以联系对应的技术支持，我们也会在不断满足用户的需求的同时扩展我们的示例库。
cd build/examples/NetCam
./net_cam
运行内部话题监听，可以帮助检查是否有数据接收，数据频率是否正常等问题，其中monitor也可显示SDK内部Topic的名称，帮助快速找到对应话题。
cd build/tools
./monitor
其中，cam_1_trigger (num: 20) 表示图像触发信号，cam_1 (num: 20) 表示图像帧数据。num表示一秒内接收的图像帧数，即频率为20Hz。
# 创建工作空间目录
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
# 下载代码
git clone git@github.com:InfiniteSenseLab/SimpleSync.git
# 返回工作空间根目录
cd ~/ros2_ws
# 编译工作空间
colcon build
# 加载环境变量
source install/setup.bash
# 找到运行节点
cd install/infinite_sense/lib/net_cam
# 启动ROS2节点
./net_cam_ros2
ros2 topic echo /imu_1时间同步板搭载 ICM42688P 6轴 IMU，支持姿态解算，适用于非紧耦合方案应用，可在 rviz2 中查看。
sudo apt-get install ros-${ROS_DISTRO}-imu-tools
# 启动rivz2
rviz2
# rviz2 中订阅话题 /imu_1
在正确连接传感器以及正确运行SDK的状态下，传感器将会被正确同步，可以通过如下一些方法进行验证
方法1：
- 设置相机的频率为低频，例如1Hz, 然后多个相机平行放置
- 使用相机软件打开多路相机
- 在镜头前晃动物体，观察采集得到的多张图像中运动物体的位置是否一致。
方法2：
- 设置相机的频率为低频，例如1Hz, 然后多个相机平行放置
- 使用相机软件打开多路相机
- 用手机或者其他设备显示一个快速变化的时钟，观察多个相机拍到的时间是否是一致的
- 连接雷达和同步板的线
- 运行SDK
- 打开雷达的软件或者上位机查看同步状态
这里以MID360为例，正确同步的状态如下
这里必须要引入ROS等时间统计工具，这里以ROS1为例。
- 录制相机和雷达时间戳
rosbag record --use-header-time -O lidar_camera /相机topic /雷达topic
- 查看时间戳
rqt_bag lidar_camera.bag
- 雷达和相机的时间是一致且对齐的
类似下图：
为了更好的适配不同的相机，我们支持自定义相机，这里以opencv 读取 video 设备为例，解读自定义相机在ros2下的写法。
在 example 文件夹下新建 VideoCam 文件夹，包括文件
- CMakeLists.txt
- main_ros2.cpp
- video_cam.h
- video_cam.cpp
四个文件
往 example 文件夹下的CMakeLists.txt 文件添加对应路径
cmake_minimum_required(VERSION 3.10)
project(example)
add_subdirectory(NetCam)
add_subdirectory(VideoCam)
add_subdirectory(CustomCam) # 添加的路径
在video_cam.h 文件中包含代码如下,其中重载函数就是我们需要实现的部分
#pragma once
#include "infinite_sense.h"
#include <opencv2/opencv.hpp>
#include <atomic>
#include <thread>
#include <vector>
#include <mutex>
namespace infinite_sense {
// 继承自 Sensor 类，会自动调用初始化，开始，停止等函数
class VideoCam final : public Sensor {
public:
~VideoCam() override;
bool Initialization() override; // 相机初始化函数
void Stop() override; // 相机停止采集函数
void Start() override;// 相机参数函数
private:
// 具体读取相机的实现
void Receive(void* handle, const std::string&) override;
// opencv 读取 video 设备的函数
cv::VideoCapture cap_;
// 是否初始化的标识
std::atomic<bool> is_initialized_{false};
// 读取图像设备的多线程，一个线程读取一个相机
std::mutex cap_mutex_;
std::vector<std::thread> cam_threads;
};
} // namespace infinite_sense
在video_cam.cpp 文件中写入如下代码
#include "video_cam.h"
#include "infinite_sense.h"
namespace infinite_sense {
// 调用停止读取函数
VideoCam::~VideoCam() { Stop(); }
// 初始化摄像头
bool VideoCam::Initialization() {
std::lock_guard<std::mutex> lock(cap_mutex_);
if (!cap_.open(0)) { // 打开默认摄像头
// 打开失败报错
std::cerr << "[OpenCVCam] Failed to open camera!" << std::endl;
return false;
}
// 设置相机帧率，注意这里的帧率需要是 1. 摄像头支持的帧率 2. 尽量大于触发帧率，这样才能保证时间戳对上
cap_.set(cv::CAP_PROP_FPS, 30);
is_initialized_ = true;// 初始化标识
std::cout << "[OpenCVCam] Camera initialized." << std::endl;
return true;
}
// 停止读取函数
void VideoCam::Stop() {
is_running = false;// 初始化标识
{
std::lock_guard<std::mutex> lock(cap_mutex_);
if (cap_.isOpened()) {
cap_.release();// 释放摄像头设备
std::cout << "[OpenCVCam] Camera released." << std::endl;
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
for (auto& t : cam_threads) {
if (t.joinable()) {
t.join();
}
}
// 关闭线程
cam_threads.clear();
cam_threads.shrink_to_fit();
}
// 相机读取函数
void VideoCam::Receive(void *handle, const std::string &name) {
// 自定义的相机类，包含1. 图像数据 2. 时间戳 3. 相机名称
CamData cam_data;
// 统一的消息管理类
Messenger &messenger = Messenger::GetInstance();
// 初始化完成，运行的状态中
while (is_running) {
// 1. 读取相机中的一帧图像
cv::Mat frame_bgr;
{
std::lock_guard<std::mutex> lock(cap_mutex_);
if (!cap_.isOpened() || !cap_.read(frame_bgr)) {
std::cerr << "[OpenCVCam] Failed to read frame!" << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(5));
continue;
}
}
// 这里的time_stamp_us是相机触发时间，调用 GET_LAST_TRIGGER_STATUS(params[name], time)) 获取name 对应设备最新触发时间
if (params.find(name) == params.end()) {
LOG(ERROR) << "cam: " << name << " not found!";
} else {
if (uint64_t time; GET_LAST_TRIGGER_STATUS(params[name], time)) {
cam_data.time_stamp_us = time;
} else {
LOG(ERROR) << "Trigger cam: " << name << " not found!";
}
}
// 相机的名称
cam_data.name = name;
// 相机的数据，直接把原始数据发送
cam_data.image = GMat(frame_bgr.rows, frame_bgr.cols,
GMatType<uint8_t, 3>::Type, frame_bgr.data).Clone();
messenger.PubStruct(name, &cam_data, sizeof(cam_data));
std::this_thread::sleep_for(std::chrono::milliseconds{2});
}
}
// 开始读取函数
void VideoCam::Start() {
if (!is_initialized_) {
std::cerr << "[OpenCVCam] Start failed: not initialized." << std::endl;
return;
}
// 运行标识位
is_running = true;
// 开启一个线程并告诉其相机名称
std::string name = "video_cam";
cam_threads.emplace_back(&VideoCam::Receive, this, nullptr, name);
std::cout << "[OpenCVCam] Camera capture started." << std::endl;
}
} // namespace infinite_sense
之后调用所提供的video_camera 类
#include "infinite_sense.h"
#include "video_cam.h"
#include "rclcpp/rclcpp.hpp"
#include <image_transport/image_transport.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/msg/imu.hpp>
// 继承自 rclcpp::Nod
class CamDriver final : public rclcpp::Node {
public:
CamDriver()
: Node("ros2_cam_driver"), node_handle_(std::shared_ptr<CamDriver>(this, [](auto *) {})), transport_(node_handle_) {
// 相机名称，注意和video_cam.cpp 中的相机名称保持一致
std::string camera_name_1 = "video_cam";
// IMU 设备名称，自定义即可
const std::string imu_name = "imu_1";
// 使用串口设备进行同步，这里可以选择不同的设备
synchronizer_.SetUsbLink("/dev/ttyACM0", 921600);
// synchronizer_.SetNetLink("192.168.1.188", 8888);
// 构建 VideoCam 类
const auto video_cam = std::make_shared<infinite_sense::VideoCam>();
// 重要！！！注册相机名称video_cam， 并且绑定 CAM_1 接口，也就是 video_cam对应的设备必须要接到 CAM_1
video_cam->SetParams({{camera_name_1, infinite_sense::CAM_1}
});
// 同步类设置添加相机类
synchronizer_.UseSensor(video_cam);
// 逐个调用开始函数
synchronizer_.Start();
// IMU 和 图像信息的发布函数
imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_name, 10);
img1_pub_ = transport_.advertise(camera_name_1, 10);
// SDK 获取IMU和图像信息之后的回调函数
{
using namespace std::placeholders;
infinite_sense::Messenger::GetInstance().SubStruct(imu_name, std::bind(&CamDriver::ImuCallback, this, _1, _2));
infinite_sense::Messenger::GetInstance().SubStruct(camera_name_1,
std::bind(&CamDriver::ImageCallback1, this, _1, _2));
}
}
// SDK 获取IMU信息后的回调函数
void ImuCallback(const void *msg, size_t) const {
// 按照ROS2的格式发布的IMU数据
const auto *imu_data = static_cast<const infinite_sense::ImuData *>(msg);
sensor_msgs::msg::Imu imu_msg;
// 得到的时间单位us,需要转换为ms
imu_msg.header.stamp = rclcpp::Time(imu_data->time_stamp_us * 1000);
imu_msg.header.frame_id = "map";
imu_msg.linear_acceleration.x = imu_data->a[0];
imu_msg.linear_acceleration.y = imu_data->a[1];
imu_msg.linear_acceleration.z = imu_data->a[2];
imu_msg.angular_velocity.x = imu_data->g[0];
imu_msg.angular_velocity.y = imu_data->g[1];
imu_msg.angular_velocity.z = imu_data->g[2];
imu_msg.orientation.w = imu_data->q[0];
imu_msg.orientation.x = imu_data->q[1];
imu_msg.orientation.y = imu_data->q[2];
imu_msg.orientation.z = imu_data->q[3];
imu_pub_->publish(imu_msg);
}
// SDK 获取图像信息后的回调函数
void ImageCallback1(const void *msg, size_t) const {
// 原始的图像数据
const auto *cam_data = static_cast<const infinite_sense::CamData *>(msg);
std_msgs::msg::Header header;
// 得到的时间单位us,需要转换为ms
header.stamp = rclcpp::Time(cam_data->time_stamp_us * 1000);
header.frame_id = "map";
// 构造opencv图像，这里是彩色图片因此是 CV_8UC3 和 bgr8
const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC3, cam_data->image.data);
const sensor_msgs::msg::Image::SharedPtr image_msg = cv_bridge::CvImage(header, "bgr8", image_mat).toImageMsg();
img1_pub_.publish(image_msg);
}
private:
infinite_sense::Synchronizer synchronizer_;
SharedPtr node_handle_;
image_transport::ImageTransport transport_;
image_transport::Publisher img1_pub_,img2_pub_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
};
// main 函数
int main(const int argc, char *argv[]) {
rclcpp::init(argc, argv);
// 构造 CamDriver 类，用于读取相机、IMU等信息
rclcpp::spin(std::make_shared<CamDriver>());
return 0;
}
最后，整体的CMakeLists.txt文件如下
cmake_minimum_required(VERSION 3.16)
project(video_cam VERSION 1.0)
message(STATUS "System Processor: ${CMAKE_SYSTEM_PROCESSOR}")
find_package(Threads REQUIRED)
find_package(OpenCV QUIET)
if(OpenCV_FOUND)
message(STATUS "Found OpenCV version: ${OpenCV_VERSION}")
message(STATUS "OpenCV include dirs: ${OpenCV_INCLUDE_DIRS}")
message(STATUS "OpenCV libraries: ${OpenCV_LIBS}")
# **************************编译普通节点*****************************
add_executable(${PROJECT_NAME}
main.cpp
video_cam.cpp
video_cam.h
)
target_link_directories(${PROJECT_NAME} PRIVATE )
target_link_libraries(${PROJECT_NAME} PRIVATE
infinite_sense_core
${OpenCV_LIBS}
Threads::Threads
)
target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS})
set_target_properties(${PROJECT_NAME} PROPERTIES INSTALL_RPATH "$ORIGIN")
# **************************编译ROS2节点*****************************
find_package(rclcpp QUIET)
if (NOT rclcpp_FOUND)
message(STATUS "rclcpp not found. ROS2 demo will not be built")
else ()
message(STATUS "rclcpp found. ROS2 demo will be built")
find_package(ament_cmake REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(image_transport REQUIRED)
find_package(OpenCV REQUIRED)
find_package(cv_bridge REQUIRED)
add_executable(${PROJECT_NAME}_ros2
main_ros2.cpp
video_cam.cpp
video_cam.h
)
target_link_directories(${PROJECT_NAME}_ros2 PRIVATE ${OpenCV_LIBS})
target_include_directories(${PROJECT_NAME}_ros2 PUBLIC ${OpenCV_INCLUDE_DIRS})
target_link_libraries(${PROJECT_NAME}_ros2
infinite_sense_core
${OpenCV_LIBRARIES}
)
set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
ament_target_dependencies(${PROJECT_NAME}_ros2
rclcpp
std_msgs
sensor_msgs
image_transport
OpenCV
cv_bridge
)
install(TARGETS ${PROJECT_NAME}_ros2
DESTINATION lib/${PROJECT_NAME}
)
install(TARGETS infinite_sense_core
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
endif ()
# **************************编译ROS1节点*****************************
find_package(catkin QUIET)
if (NOT catkin_FOUND)
message(STATUS "catkin not found, ROS1 demo will not be built")
else ()
message(STATUS "catkin found. ROS1 demo will be built")
find_package(catkin REQUIRED COMPONENTS
roscpp
sensor_msgs
cv_bridge
image_transport
geometry_msgs
)
file(GLOB_RECURSE SOURCE_FILES
main_ros.cpp
video_cam.cpp
video_cam.h
)
add_executable(${PROJECT_NAME}_ros ${SOURCE_FILES})
target_link_directories(${PROJECT_NAME}_ros PRIVATE ${OpenCV_LIBS})
target_link_libraries(${PROJECT_NAME}_ros
infinite_sense_core
Threads::Threads
${catkin_LIBRARIES}
)
target_include_directories(${PROJECT_NAME}_ros PRIVATE
${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}
)
target_compile_definitions(${PROJECT_NAME}_ros PUBLIC -DROS_PLATFORM)
endif ()
endif()重新编译整个工程即可生成针对特定相机型号的可执行文件，执行改文件即可。