feat: add support for timestamp clock type

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -87,6 +87,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
 
   OBDeviceAccessMode stringToAccessMode(const std::string& mode_str);
   std::string accessModeToString(OBDeviceAccessMode mode);
+  OBClockType timestampClockTypeFromString(const std::string& clock_type_str);
+  std::string timestampClockTypeToString(OBClockType clock_type);
 
  private:
   const rclcpp::NodeOptions node_options_;
@@ -131,6 +133,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
   int connection_delay_ = 100;
   bool enable_sync_host_time_ = true;
   std::chrono::milliseconds time_sync_period_{6000};
+  std::string timestamp_clock_type_str_;
   std::string preset_firmware_path_;
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
   std::chrono::time_point<std::chrono::system_clock> start_time_;

orbbec_camera/launch/astra.launch.py

@@ -68,6 +68,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
         DeclareLaunchArgument('use_hardware_time', default_value='false'),
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_depth_scale', default_value='true'),

orbbec_camera/launch/astra2.launch.py

@@ -105,6 +105,7 @@ def generate_launch_description():
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
         DeclareLaunchArgument("time_domain", default_value="global"),
+        DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
     ]

orbbec_camera/launch/dabai_a.launch.py

@@ -207,6 +207,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
         DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),

orbbec_camera/launch/dabai_al.launch.py

@@ -209,6 +209,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
         DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),

orbbec_camera/launch/dabai_dcw2.launch.py

@@ -82,6 +82,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_lut_noise_removal_filter', default_value='false'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
         DeclareLaunchArgument('use_hardware_time', default_value='false'),
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_depth_scale', default_value='true'),

orbbec_camera/launch/dabai_max_pro.launch.py

@@ -76,6 +76,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_mgc_noise_removal_filter', default_value='false'),
         DeclareLaunchArgument('enable_lut_noise_removal_filter', default_value='false'),
         DeclareLaunchArgument('use_hardware_time', default_value='false'),
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('align_mode', default_value='HW'),

orbbec_camera/launch/femto.launch.py

@@ -83,6 +83,7 @@ def generate_launch_description():
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
         DeclareLaunchArgument("time_domain", default_value="device"),
+        DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('device_preset', default_value='Custom'),

orbbec_camera/launch/femto_bolt.launch.py

@@ -111,6 +111,7 @@ def generate_launch_description():
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
         DeclareLaunchArgument("time_domain", default_value="global"),
+        DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('device_preset', default_value='Custom'),

orbbec_camera/launch/femto_mega.launch.py

@@ -117,6 +117,7 @@ def generate_launch_description():
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
         DeclareLaunchArgument("time_domain", default_value="global"),
+        DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('device_preset', default_value='Custom'),

orbbec_camera/launch/gemini2.launch.py

@@ -133,6 +133,7 @@ def generate_launch_description():
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
         DeclareLaunchArgument("time_domain", default_value="global"),
+        DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
     ]

orbbec_camera/launch/gemini210.launch.py

@@ -131,6 +131,7 @@ def generate_launch_description():
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
         DeclareLaunchArgument("time_domain", default_value="global"),
+        DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
     ]

orbbec_camera/launch/gemini2L.launch.py

@@ -184,6 +184,7 @@ def generate_launch_description():
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
         DeclareLaunchArgument("time_domain", default_value="global"),
+        DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument("enable_frame_timestamp_csv", default_value="false"),
         DeclareLaunchArgument("frame_timestamp_csv_file", default_value=""),
         DeclareLaunchArgument("config_file_path", default_value=""),

orbbec_camera/launch/gemini305.launch.py

@@ -272,6 +272,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
         DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),

orbbec_camera/launch/gemini305_g.launch.py

@@ -272,6 +272,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
         DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),

orbbec_camera/launch/gemini345.launch.py

@@ -206,6 +206,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
         DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),

orbbec_camera/launch/gemini345_lg.launch.py

@@ -209,6 +209,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
         DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),

orbbec_camera/launch/gemini435_le.launch.py

@@ -240,6 +240,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
         DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),

orbbec_camera/launch/gemini_330_series.launch.py

@@ -257,6 +257,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
         DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),

orbbec_camera/launch/gemini_330_series_low_cpu.launch.py

@@ -256,6 +256,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
         DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),

orbbec_camera/launch/gemini_intra_process_demo_launch.py

@@ -162,6 +162,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_sync_host_time', default_value='true'),
         DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='device'),
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),

orbbec_camera/launch/lidar.launch.py

@@ -181,6 +181,11 @@ def generate_launch_description():
             default_value='device',  # global, device, system
             description='Timestamp domain: device (device clock), global (time sync), or system (host clock).'
         ),
+        DeclareLaunchArgument(
+            'timestamp_clock_type',
+            default_value='',
+            description='SDK timestamp clock type: realtime or monotonic, default is realtime.'
+        ),
         DeclareLaunchArgument(
             'config_file_path',
             default_value='',

orbbec_camera/launch/orbbec_camera.launch.py

@@ -28,6 +28,7 @@ def generate_launch_arguments():
         DeclareLaunchArgument('camera_model', default_value=default_camera_model),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('log_level', default_value='none'),
+        DeclareLaunchArgument('timestamp_clock_type', default_value=''),  # realtime or monotonic, default is realtime.
         #multi-device sync param
         DeclareLaunchArgument('camera_name', default_value='camera'),
         DeclareLaunchArgument('usb_port', default_value=''),

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -263,6 +263,19 @@ void OBCameraNodeDriver::init() {
   } else {
     ctx_ = std::make_unique<ob::Context>(config_path_.c_str());
   }
+  timestamp_clock_type_str_ = declare_parameter<std::string>("timestamp_clock_type", "");
+  if (!timestamp_clock_type_str_.empty()) {
+    auto timestamp_clock_type = timestampClockTypeFromString(timestamp_clock_type_str_);
+    try {
+      ctx_->setTimestampClockType(timestamp_clock_type);
+      auto actual_timestamp_clock_type = ctx_->getTimestampClockType();
+      RCLCPP_INFO_STREAM(logger_, "Set timestamp clock type to "
+                                      << timestampClockTypeToString(actual_timestamp_clock_type));
+    } catch (const ob::Error &e) {
+      RCLCPP_WARN_STREAM(logger_, "Failed to set SDK timestamp clock type: "
+                                      << orbbec_camera::formatObErrorWithStatus(e));
+    }
+  }
   applyForceIpConfig();
 
   device_type_ = declare_parameter<std::string>("device_type", "camera");
@@ -1577,6 +1590,33 @@ OBDeviceAccessMode OBCameraNodeDriver::stringToAccessMode(const std::string &mod
   }
 }
 
+OBClockType OBCameraNodeDriver::timestampClockTypeFromString(const std::string &clock_type_str) {
+  std::string lower_type;
+  std::transform(clock_type_str.begin(), clock_type_str.end(), std::back_inserter(lower_type),
+                 [](auto ch) { return tolower(ch); });
+
+  if (lower_type == "realtime") {
+    return OB_CLOCK_TYPE_REALTIME;
+  }
+  if (lower_type == "monotonic") {
+    return OB_CLOCK_TYPE_MONOTONIC;
+  }
+
+  RCLCPP_WARN_STREAM(logger_,
+                     "Unknown timestamp_clock_type: " << clock_type_str << ", using realtime");
+  return OB_CLOCK_TYPE_REALTIME;
+}
+
+std::string OBCameraNodeDriver::timestampClockTypeToString(OBClockType clock_type) {
+  switch (clock_type) {
+    case OB_CLOCK_TYPE_MONOTONIC:
+      return "monotonic";
+    case OB_CLOCK_TYPE_REALTIME:
+    default:
+      return "realtime";
+  }
+}
+
 std::string OBCameraNodeDriver::accessModeToString(OBDeviceAccessMode mode) {
   switch (mode) {
     case OB_DEVICE_EXCLUSIVE_ACCESS: