support G335Lg multi camera sync

orbbec_camera/src/ob_camera_node.cpp

@@ -924,6 +924,69 @@ void OBCameraNode::stopIMU() {
   }
 }
 
+// cs_param_t rd_par = {0, 0}, param = {1, 3000}; //30
+int OBCameraNode::openSocSyncPwmTrigger(uint16_t fps) {
+    const char *devicePath           = DEVICE_PATH;
+    const int   TRIGGER_MODE_ENABLE  = 1;
+    const int   TRIGGER_MODE_DISABLE = 0;
+
+    int        ret    = -1;
+    cs_param_t param  = { TRIGGER_MODE_ENABLE, fps };
+    cs_param_t rd_par = { TRIGGER_MODE_DISABLE, 0 };
+
+    if(access(devicePath, F_OK) != 0) {
+        std::cerr << "Device node " << devicePath << " does not exist." << std::endl;
+        return ret;
+    }
+    gmsl_trigger_fd_ = open(DEVICE_PATH, O_RDWR);
+    if(gmsl_trigger_fd_ < 0) {
+        perror("open device failed\n");
+        return gmsl_trigger_fd_;
+    }
+
+    std::cout << "Written param mode=" << param.mode << ", fps=" << param.fps << std::endl;
+    ret = write(gmsl_trigger_fd_, &param, sizeof(param));
+    if(ret < 0) {
+        perror("write device failed\n");
+        close(gmsl_trigger_fd_);
+        return ret;
+    }
+
+    ret = read(gmsl_trigger_fd_, &rd_par, sizeof(rd_par));
+    if(ret < 0) {
+        perror("read device failed\n");
+        close(gmsl_trigger_fd_);
+        return ret;
+    }
+    std::cout << "Read param mode=" << rd_par.mode << ", fps=" << rd_par.fps << std::endl;
+
+    std::cout << "Start hardware triggering..." << std::endl;
+
+    return 0;
+}
+int OBCameraNode::closeSocSyncPwmTrigger() {
+    if(gmsl_trigger_fd_ >= 0) {
+        close(gmsl_trigger_fd_);
+        gmsl_trigger_fd_ = -1;  // Reset file descriptors
+        std::cout << "close camSync success" << std::endl;
+        return 0;
+    }
+    return -1;
+}
+
+void OBCameraNode::startGmslTrigger() {
+  if(gmsl_trigger_fps_ > 0 && enable_gmsl_trigger_) {
+    RCLCPP_WARN_STREAM(logger_, "Start HardwareTrigger by soc-trigger-source. gmsl_trigger_fps_: " << gmsl_trigger_fps_);
+    openSocSyncPwmTrigger(gmsl_trigger_fps_);
+  }
+  else {
+    RCLCPP_WARN_STREAM(logger_, "Start HardwareTrigger by soc-trigger-source. gmsl_trigger_fps_ illegal: " << gmsl_trigger_fps_);
+  }
+}
+void OBCameraNode::stopGmslTrigger() {
+  closeSocSyncPwmTrigger();
+}
+
 void OBCameraNode::setupDefaultImageFormat() {
   format_[DEPTH] = OB_FORMAT_Y16;
   format_str_[DEPTH] = "Y16";
@@ -1131,6 +1194,8 @@ void OBCameraNode::getParameters() {
   long software_trigger_period = 33;
   setAndGetNodeParameter<long>(software_trigger_period, "software_trigger_period", 33);
   software_trigger_period_ = std::chrono::milliseconds(software_trigger_period);
+  setAndGetNodeParameter<int>(gmsl_trigger_fps_, "gmsl_trigger_fps", 3000);
+  setAndGetNodeParameter<bool>(enable_gmsl_trigger_, "enable_gmsl_trigger", false);
 }
 
 void OBCameraNode::setupTopics() {

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -104,6 +104,7 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
     reset_device_cond_.notify_all();
     reset_device_thread_->join();
   }
+  ob_camera_node_->stopGmslTrigger();
 }
 
 void OBCameraNodeDriver::init() {
@@ -415,6 +416,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
 
   ob_camera_node_->startIMU();
   ob_camera_node_->startStreams();
+
   device_connected_ = true;
   device_info_ = device_->getDeviceInfo();
   serial_number_ = device_info_->getSerialNumber();
@@ -490,6 +492,11 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
     end_time = std::chrono::high_resolution_clock::now();
     time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
     RCLCPP_INFO_STREAM(logger_, "Initialize device cost " << time_cost.count() << " ms");
+
+    auto pid = device->getDeviceInfo()->getPid();
+    if (GEMINI_335LG_PID == pid) {
+      ob_camera_node_->startGmslTrigger();
+    }
   } catch (ob::Error &e) {
     RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.getMessage());
     start_device_failed = true;