Skip to content

SDK运行与同步

Jump to bottom
Lin Yicheng edited this page Jun 29, 2025 · 40 revisions

下载安装

sudo apt-get install libzmq3-dev
git clone git@github.com:InfiniteSenseLab/SimpleSensorSync.git -b main
cd SimpleSync
mkdir build && cd build
cmake ..
make -j

例程

基础例程

运行精简版本Demo

cd build/examples/NetCam
./net_cam

image

内部话题监听节点(可选)

运行内部话题监听，可以帮助检查是否有数据接收，数据频率是否正常等问题，其中monitor也可显示SDK内部Topic的名称，帮助快速找到对应话题。

cd build/tools
./monitor

monitor

其中，cam_1_trigger (num: 20) 表示图像触发信号，cam_1 (num: 20) 表示图像帧数据。num表示一秒内接收的图像帧数，即频率为20Hz。

ROS2例程

创建空间并运行

# 创建工作空间目录
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

image

打印原始 IMU 数据

ros2 topic echo /imu_1

查看姿态（基于 ICM42688P 航姿解算）

时间同步板搭载 ICM42688P 6轴 IMU，支持姿态解算，适用于非紧耦合方案应用，可在 rviz2 中查看。

  sudo apt-get install ros-${ROS_DISTRO}-imu-tools
  # 启动rivz2
  rviz2
  # rviz2 中订阅话题 /imu_1

ROS1例程

传感器同步效果验证

在正确连接传感器以及正确运行SDK的状态下，传感器将会被正确同步，可以通过如下一些方法进行验证

1. 多相机之间的同步验证

方法1：

  1. 设置相机的频率为低频，例如1Hz, 然后多个相机平行放置
  2. 使用相机软件打开多路相机
  3. 在镜头前晃动物体，观察采集得到的多张图像中运动物体的位置是否一致。

Screenshot from 2025-06-22 13-10-45

方法2：

  1. 设置相机的频率为低频，例如1Hz, 然后多个相机平行放置
  2. 使用相机软件打开多路相机
  3. 用手机或者其他设备显示一个快速变化的时钟，观察多个相机拍到的时间是否是一致的

2. 雷达同步验证

  1. 连接雷达和同步板的线
  2. 运行SDK
  3. 打开雷达的软件或者上位机查看同步状态

这里以MID360为例，正确同步的状态如下

image

3. 相机和雷达时间同步

这里必须要引入ROS等时间统计工具，这里以ROS1为例。

  1. 录制相机和雷达时间戳
rosbag record -O lidar_camera /相机topic /雷达topic
  1. 查看时间戳
rqt_bag lidar_camera.bag
  1. 雷达和相机的时间是一致且对齐的

类似下图： image

自定义相机

为了更好的适配不同的相机，我们支持自定义相机，这里以opencv 读取 video 设备为例，解读自定义相机在ros2下的写法。

1. 准备工作

在 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) # 添加的路径

2. 参考示例完善代码

在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()

重新编译整个工程即可生成针对特定相机型号的可执行文件，执行改文件即可。

Clone this wiki locally