add ROS2 node here

Author: KingRichord

example/GigeCam/main.cpp

@@ -6,6 +6,7 @@ using namespace infinite_sense;
 // 自定义回调函数
 void ImuCallback(const void* msg, size_t) {
   const auto* imu_data = static_cast<const ImuData*>(msg);
+  LOG(INFO) << imu_data->time_stamp_us << " " << "Accel: " << imu_data->a[0] << " " << imu_data->a[1] << " " << imu_data->a[2] << " Gyro: " << imu_data->g[0] << " " << imu_data->g[1] << " " << imu_data->g[2] << " Temp: " << imu_data->temperature;
   // 处理IMU数据
 }
 // 自定义回调函数

example/GigeCam/main_ros2.cpp

@@ -1,28 +1,69 @@
 #include "infinite_sense.h"
+#include "mv_cam.h"
 #include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/string.hpp"
-#include "sensor_msgs/msg/image.hpp"
 #include "sensor_msgs/image_encodings.hpp"
 #include "image_transport/image_transport.hpp"
-#include "opencv2/highgui/highgui.hpp"
-#include "opencv2/imgproc.hpp"
-class InfiniteSenseGigeDriver : public rclcpp::Node {
- public:
-  InfiniteSenseGigeDriver()
-      : Node("gige_driver"),
-        node_handle_(),
-        transport_(node_handle_),
-        img_pub_(transport_.advertise("camera/image_raw", 1)) {}
-
- private:
-  rclcpp::Node::SharedPtr node_handle_;
-  image_transport::ImageTransport transport_;
-  image_transport::Publisher img_pub_;
+#include "sensor_msgs/msg/imu.hpp"
+#include <cv_bridge/cv_bridge.h>
+class CamDriver final : public rclcpp::Node {
+public:
+    CamDriver()
+        : Node("gige_driver"),
+          node_handle_(std::shared_ptr<CamDriver>(this, [](auto *) {})),
+          transport_(node_handle_) {
+        synchronizer_.SetUsbLink("/dev/ttyACM0", 921600);
+        const auto mv_cam = std::make_shared<infinite_sense::MvCam>();
+        mv_cam->SetParams({{"cam_1", infinite_sense::CAM_1}});
+        synchronizer_.UseSensor(mv_cam);
+        synchronizer_.Start();
+        imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
+        img_pub_ = transport_.advertise("image_raw", 10);
+        {
+            using namespace std::placeholders;
+            infinite_sense::Messenger::GetInstance().SubStruct("imu_1", std::bind(&CamDriver::ImuCallback, this, _1, _2));
+            infinite_sense::Messenger::GetInstance().SubStruct("cam_1", std::bind(&CamDriver::ImageCallback, this, _1, _2));
+        }
+    }
+    void ImuCallback(const void* msg, size_t) const {
+        const auto* imu_data = static_cast<const infinite_sense::ImuData*>(msg);
+        sensor_msgs::msg::Imu imu_msg;
+        rclcpp::Time stamp(static_cast<uint64_t>(imu_data->time_stamp_us) * 1000); // 微秒 -> 纳秒
+        imu_msg.header.stamp = stamp;
+        imu_msg.header.frame_id = "imu_link";
+        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);
+    }
+    void ImageCallback(const void* msg, size_t) {
+        const auto* cam_data = static_cast<const infinite_sense::CamData*>(msg);
+        // 构造 ROS2 图像消息
+        std_msgs::msg::Header header;
+        header.stamp = rclcpp::Time(cam_data->time_stamp_us * 1000);  // us -> ns
+        header.frame_id = "camera_link";
+        const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC1, cam_data->image.data);
+        sensor_msgs::msg::Image::SharedPtr image_msg =
+            cv_bridge::CvImage(header, "mono8", image_mat).toImageMsg();
+        img_pub_.publish(image_msg);
+    }
+private:
+    infinite_sense::Synchronizer synchronizer_;
+    rclcpp::Node::SharedPtr node_handle_;
+    image_transport::ImageTransport transport_;
+    image_transport::Publisher img_pub_;
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
 };
 
-int main(int argc, char* argv[]) {
-  rclcpp::init(argc, argv);
-  rclcpp::shutdown();
-
-  return 0;
-}
+int main(const int argc, char *argv[]) {
+    printf("Starting GigeCam ROS2 driver...\n");
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<CamDriver>());
+    return 0;
+}

firmware/SDK2_FIRMWARE_2025-6-7.uf2

@@ -7,6 +7,9 @@
 #include <unordered_set>
 #include <unordered_map>
 #include <functional>
+
+#include "config.h"
+
 namespace infinite_sense {
 class Messenger {
  public:
@@ -22,7 +25,7 @@ class Messenger {
   void Sub(const std::string& topic, const std::function<void(const std::string&)>& callback);
   void SubStruct(const std::string& topic, const std::function<void(const void*, size_t)>& callback);
 
- private:
+private:
   Messenger();
   ~Messenger();
   void CleanUp();

infinite_sense_core/include/messenger.h

@@ -46,7 +46,7 @@ void Messenger::Pub(const std::string& topic, const std::string& metadata) {
   }
 }
 
-void Messenger::PubStruct(const std::string& topic, const void* data, size_t size) {
+void Messenger::PubStruct(const std::string& topic, const void* data, const size_t size) {
   try {
     publisher_.send(zmq::buffer(topic), zmq::send_flags::sndmore);
     publisher_.send(zmq::buffer(data, size), zmq::send_flags::dontwait);
@@ -56,12 +56,11 @@ void Messenger::PubStruct(const std::string& topic, const void* data, size_t siz
 }
 
 void Messenger::Sub(const std::string& topic, const std::function<void(const std::string&)>& callback) {
-  sub_threads_.emplace_back([=, this]() {
+  sub_threads_.emplace_back([this, topic, callback]() {
     try {
       zmq::socket_t subscriber(context_, zmq::socket_type::sub);
       subscriber.connect(endpoint_);
       subscriber.set(zmq::sockopt::subscribe, topic);
-
       while (true) {
         zmq::message_t topic_msg, data_msg;
         if (!subscriber.recv(topic_msg) || !subscriber.recv(data_msg)) {
@@ -82,9 +81,9 @@ void Messenger::Sub(const std::string& topic, const std::function<void(const std
 }
 
 void Messenger::SubStruct(const std::string& topic, const std::function<void(const void*, size_t)>& callback) {
-  sub_threads_.emplace_back([=, this]() {
+  sub_threads_.emplace_back([this, topic, callback]() {
     try {
-      zmq::context_t context = zmq::context_t(1);
+      auto context = zmq::context_t(1);
       zmq::socket_t subscriber(context, zmq::socket_type::sub);
       subscriber.connect(endpoint_);
       subscriber.set(zmq::sockopt::subscribe, topic);