support G335Lg multi camera sync

orbbec_camera/config/camera_params.yaml

@@ -1,33 +1,33 @@
 # common params
-depth_registration: false
+depth_registration: true
 enable_point_cloud: false
 enable_colored_point_cloud: false
 device_preset: "High Accuracy"
-laser_on_off_mode: 1 # 0: off, 1: on-off, 1: off-on
+laser_on_off_mode: 0 # 0: off, 1: on-off, 1: off-on
 time_domain: "global" # global, device, system
 enable_sync_host_time: false
-frames_per_trigger: 2
+frames_per_trigger: 1
 
 # When 3D reconstruction mode is enabled:
 # - The laser will switch to on-off mode
 # - IR images without the laser will be used for SLAM localization
 # - Depth images with the laser will be used because they provide better depth quality
-enable_3d_reconstruction_mode: true
+enable_3d_reconstruction_mode: false
 
 # color params
 enable_color: true
 color_width: 640
 color_height: 480
-color_fps: 90
+color_fps: 30
 color_format: "YUYV"
-enable_color_auto_exposure: false
+enable_color_auto_exposure: true
 color_exposure: 50 # 5ms
 color_gain: -1 # -1 default
 
 # depth params
 depth_width: 640
 depth_height: 480
-depth_fps: 90
+depth_fps: 30
 depth_format: "Y16"
 
 # ir exposure
@@ -39,12 +39,12 @@ ir_gain: 40
 enable_left_ir: true
 left_ir_width: 640
 left_ir_height: 480
-left_ir_fps: 90
+left_ir_fps: 30
 left_ir_format: "Y8"
 
 #right ir params
 enable_right_ir: true
 right_ir_width: 640
 right_ir_height: 480
-right_ir_fps: 90
+right_ir_fps: 30
 right_ir_format: "Y8"

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -63,6 +63,7 @@
 #include "orbbec_camera/image_publisher.h"
 #include "jpeg_decoder.h"
 #include <std_msgs/msg/string.hpp>
+#include <fcntl.h>
 
 #if defined(ROS_JAZZY) || defined(ROS_IRON)
 #include <cv_bridge/cv_bridge.hpp>
@@ -94,6 +95,8 @@
   (static_cast<std::ostringstream&&>(std::ostringstream() << getNamespaceStr() << "_odom_frame")) \
       .str()
 
+#define DEVICE_PATH "/dev/camsync"
+
 namespace orbbec_camera {
 using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo;
 using Extrinsics = orbbec_camera_msgs::msg::Extrinsics;
@@ -129,6 +132,11 @@ const std::map<OBStreamType, OBFrameType> STREAM_TYPE_TO_FRAME_TYPE = {
     {OB_STREAM_ACCEL, OB_FRAME_ACCEL},
 };
 
+typedef struct {
+    uint8_t  mode;
+    uint16_t fps;
+} cs_param_t;
+
 class OBCameraNode {
  public:
   OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device,
@@ -152,6 +160,11 @@ class OBCameraNode {
 
   void startIMU();
 
+  int openSocSyncPwmTrigger(uint16_t fps);
+  int closeSocSyncPwmTrigger();
+  void startGmslTrigger();
+  void stopGmslTrigger();
+
  private:
   struct IMUData {
     IMUData() = default;
@@ -539,6 +552,9 @@ class OBCameraNode {
   int hdr_merge_gain_1_ = -1;
   int hdr_merge_exposure_2_ = -1;
   int hdr_merge_gain_2_ = -1;
+  int gmsl_trigger_fd_ = -1;
+  int gmsl_trigger_fps_ = -1;
+  bool enable_gmsl_trigger_ = false;
 
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr filter_status_pub_;
   nlohmann::json filter_status_;

orbbec_camera/launch/gemini_330_series.launch.py

@@ -173,6 +173,8 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
+        DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
     ]
 
     def get_params(context, args):

orbbec_camera/launch/gemini_intra_process_demo_launch.py

@@ -172,6 +172,8 @@ def generate_launch_description():
         DeclareLaunchArgument('topic_type', default_value='points'),
         DeclareLaunchArgument('topic_name', default_value='/camera/depth_registered/points'),
         DeclareLaunchArgument('use_intra_process_comms', default_value='true'),
+        DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
+        DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
     ]
 
     def get_params(context, args):

orbbec_camera/launch/multi_camera.launch.py

@@ -16,21 +16,39 @@ def generate_launch_description():
         ),
         launch_arguments={
             'camera_name': 'camera_01',
-            'usb_port': 'gmsl2-2',
+            'usb_port': 'gmsl2-1',
             'device_num': '2',
-            'sync_mode': 'standalone'
+            'sync_mode': 'standalone',
+            'enable_left_ir': 'true',
+            'enable_right_ir': 'true',
         }.items()
     )
 
-    launch2_include = IncludeLaunchDescription(
+    # launch2_include = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(launch_file_dir, 'gemini_330_series.launch.py')
+    #     ),
+    #     launch_arguments={
+    #         'camera_name': 'camera_02',
+    #         'usb_port': 'gmsl2-2',
+    #         'device_num': '3',
+    #         'sync_mode': 'standalone',
+    #         'enable_left_ir': 'false',
+    #         'enable_right_ir': 'false',
+    #     }.items()
+    # )
+
+    launch3_include = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(launch_file_dir, 'gemini_330_series.launch.py')
         ),
         launch_arguments={
-            'camera_name': 'camera_02',
+            'camera_name': 'camera_03',
             'usb_port': 'gmsl2-3',
             'device_num': '2',
-            'sync_mode': 'standalone'
+            'sync_mode': 'standalone',
+            'enable_left_ir': 'true',
+            'enable_right_ir': 'true',
         }.items()
     )
 
@@ -39,7 +57,8 @@ def generate_launch_description():
     # Launch description
     ld = LaunchDescription([
         GroupAction([launch1_include]),
-        GroupAction([launch2_include]),
+        # GroupAction([launch2_include]),
+        GroupAction([launch3_include]),
     ])
 
     return ld

orbbec_camera/launch/multi_camera_synced.launch.py

@@ -18,57 +18,61 @@ def generate_launch_description():
         ),
         launch_arguments={
             "camera_name": "front_camera",
-            "usb_port": "2-6",
+            "usb_port": "gmsl2-1",
-            "device_num": "3",
+            "device_num": "2",
-            "sync_mode": "software_triggering",
+            "sync_mode": "hardware_triggering",
             "config_file_path": config_file_path,
+            "enable_gmsl_trigger": "true",
         }.items(),
     )
 
-    left_camera = IncludeLaunchDescription(
+    # left_camera = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
+    #     PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, "gemini_330_series.launch.py")
+    #         os.path.join(launch_file_dir, "gemini_330_series.launch.py")
-        ),
+    #     ),
-        launch_arguments={
+    #     launch_arguments={
-            "camera_name": "left_camera",
+    #         "camera_name": "left_camera",
-            "usb_port": "2-1.2.1",
+    #         "usb_port": "gmsl2-2",
-            "device_num": "3",
+    #         "device_num": "3",
-            "sync_mode": "hardware_triggering",
+    #         "sync_mode": "secondary",
-            "config_file_path": config_file_path,
+    #         "config_file_path": config_file_path,
-        }.items(),
+    #         "enable_gmsl_trigger": "false",
-    )
+    #     }.items(),
-    rear_camera = IncludeLaunchDescription(
+    # )
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, "gemini_330_series.launch.py")
-        ),
-        launch_arguments={
-            "camera_name": "rear_camera",
-            "usb_port": "2-3",
-            "device_num": "3",
-            "sync_mode": "hardware_triggering",
-            "config_file_path": config_file_path,
-        }.items(),
-    )
     right_camera = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(launch_file_dir, "gemini_330_series.launch.py")
         ),
         launch_arguments={
             "camera_name": "right_camera",
-            "usb_port": "2-7",
+            "usb_port": "gmsl2-3",
-            "device_num": "3",
+            "device_num": "2",
             "sync_mode": "hardware_triggering",
             "config_file_path": config_file_path,
+            "enable_gmsl_trigger": "false",
         }.items(),
     )
+    # rear_camera = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(launch_file_dir, "gemini_330_series.launch.py")
+    #     ),
+    #     launch_arguments={
+    #         "camera_name": "rear_camera",
+    #         "usb_port": "gmsl2-4",
+    #         "device_num": "3",
+    #         "sync_mode": "secondary",
+    #         "config_file_path": config_file_path,
+    #         "enable_gmsl_trigger": "false",
+    #     }.items(),
+    # )
 
     # Launch description
     ld = LaunchDescription(
         [
-            GroupAction([rear_camera]),
+          # TimerAction(period=0.5, actions=[GroupAction([rear_camera])]),
-            GroupAction([left_camera]),
+          TimerAction(period=0.5, actions=[GroupAction([right_camera])]),
-            GroupAction([right_camera]),
+          # TimerAction(period=0.5, actions=[GroupAction([left_camera])]),
-            TimerAction(period=3.0, actions=[GroupAction([front_camera])]),
+          TimerAction(period=0.5, actions=[GroupAction([front_camera])]),
           # The primary camera should be launched at last
         ]
     )

orbbec_camera/launch/multi_camera_synced_multi_save_rgbir.launch.py

@@ -0,0 +1,109 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription, GroupAction, TimerAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    # Include launch files
+    package_dir = get_package_share_directory("orbbec_camera")
+    launch_file_dir = os.path.join(package_dir, "launch")
+    config_file_dir = os.path.join(package_dir, "config")
+    config_file_path = os.path.join(config_file_dir, "camera_params.yaml")
+    front_camera = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, "gemini_330_series.launch.py")
+        ),
+        launch_arguments={
+            "camera_name": "front_camera",
+            "usb_port": "gmsl2-1",
+            "device_num": "2",
+            "sync_mode": "secondary",
+            "config_file_path": config_file_path,
+            "enable_gmsl_trigger": "true",
+        }.items(),
+    )
+    # left_camera = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(launch_file_dir, "gemini_330_series.launch.py")
+    #     ),
+    #     launch_arguments={
+    #         "camera_name": "left_camera",
+    #         "usb_port": "gmsl2-2",
+    #         "device_num": "3",
+    #         "sync_mode": "secondary",
+    #         "config_file_path": config_file_path,
+    #         "enable_gmsl_trigger": "false",
+    #     }.items(),
+    # )
+    right_camera = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, "gemini_330_series.launch.py")
+        ),
+        launch_arguments={
+            "camera_name": "right_camera",
+            "usb_port": "gmsl2-3",
+            "device_num": "2",
+            "sync_mode": "secondary",
+            "config_file_path": config_file_path,
+            "enable_gmsl_trigger": "false",
+        }.items(),
+    )
+    # rear_camera = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(launch_file_dir, "gemini_330_series.launch.py")
+    #     ),
+    #     launch_arguments={
+    #         "camera_name": "rear_camera",
+    #         "usb_port": "gmsl2-4",
+    #         "device_num": "3",
+    #         "sync_mode": "secondary",
+    #         "config_file_path": config_file_path,
+    #         "enable_gmsl_trigger": "false",
+    #     }.items(),
+    # )
+
+    multi_save_rgbir_node = Node(
+      package="orbbec_camera",
+      executable="multi_save_rgbir_node",
+      name="multi_save_rgbir_node",
+      parameters=[
+        {
+            # The port number should be filled in according to the order of the port numbers above.
+            # "usb_ports": ["gmsl2-1","gmsl2-2","gmsl2-3"],
+            "usb_ports": ["gmsl2-1","gmsl2-3"],
+            "ir_topics": [
+                "/front_camera/left_ir/image_raw",
+                # "/left_camera/left_ir/image_raw",
+                "/right_camera/left_ir/image_raw",
+                # "/rear_camera/left_ir/image_raw",
+            ],
+            "color_topics": [
+                "/front_camera/color/image_raw",
+                # "/left_camera/color/image_raw",
+                "/right_camera/color/image_raw",
+                # "/rear_camera/color/image_raw",
+            ],
+          }
+      ],
+    )
+
+    # Launch description
+    ld = LaunchDescription(
+        [
+          GroupAction([multi_save_rgbir_node]),
+          TimerAction(
+            period=0.5,
+            actions=[
+              # TimerAction(period=0.5, actions=[GroupAction([rear_camera])]),
+              TimerAction(period=0.5, actions=[GroupAction([right_camera])]),
+              # TimerAction(period=0.5, actions=[GroupAction([left_camera])]),
+              TimerAction(period=0.5, actions=[GroupAction([front_camera])]),
+              # The primary camera should be launched at last
+            ],
+          ),
+        ]
+    )
+
+    return ld

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;