From 7a4a1683c68a412edcb54f8be400ba052c603da3 Mon Sep 17 00:00:00 2001 From: Damien Miliche Date: Fri, 5 Dec 2025 17:13:50 +0100 Subject: [PATCH] Add the possibility to use a bag file instead of a physical device A bag_filename can now be specified as parameter to the orbbec_camera_node, that will be replayed using the PlaybackDevice interface. A bag_replay_loop boolean parameter is added to allow replaying the bag in a loop. The format of the bag file is the one generated when recording from the OrbbecViewer tool. --- .../orbbec_camera/ob_camera_node_driver.h | 4 ++ orbbec_camera/include/orbbec_camera/utils.h | 2 + .../launch/gemini_330_series.launch.py | 2 + orbbec_camera/src/ob_camera_node.cpp | 6 +-- orbbec_camera/src/ob_camera_node_driver.cpp | 49 +++++++++++++++++++ orbbec_camera/src/utils.cpp | 7 +++ 6 files changed, 67 insertions(+), 3 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h index 1f839c53..9182ac19 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h @@ -60,6 +60,8 @@ class OBCameraNodeDriver : public rclcpp::Node { void connectNetDevice(const std::string& net_device_ip, int net_device_port); + void openPlaybackDevice(const std::string &bagfile_path, bool bag_replay_loop); + void onDeviceConnected(const std::shared_ptr& device_list); void onDeviceDisconnected(const std::shared_ptr& device_list); @@ -119,6 +121,8 @@ class OBCameraNodeDriver : public rclcpp::Node { // net config std::string net_device_ip_; int net_device_port_ = 0; + std::string bag_filename_; + bool bag_replay_loop_ = false; int connection_delay_ = 100; bool enable_sync_host_time_ = true; std::chrono::milliseconds time_sync_period_{6000}; diff --git a/orbbec_camera/include/orbbec_camera/utils.h b/orbbec_camera/include/orbbec_camera/utils.h index e278dffa..ce975b39 100644 --- a/orbbec_camera/include/orbbec_camera/utils.h +++ b/orbbec_camera/include/orbbec_camera/utils.h @@ -210,4 +210,6 @@ cv::Mat undistortImage(const cv::Mat& image, const OBCameraIntrinsic& intrinsic, std::string getDistortionModels(OBCameraDistortion distortion); std::string calcMD5(const std::string& data); + +bool is_physical_device(std::shared_ptr device); } // namespace orbbec_camera diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index c1f4a3d8..0c1a633c 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -186,6 +186,8 @@ def generate_launch_description(): DeclareLaunchArgument('enumerate_net_device', default_value='true'), DeclareLaunchArgument('net_device_ip', default_value=''), DeclareLaunchArgument('net_device_port', default_value='0'), + DeclareLaunchArgument('bag_filename', default_value=''), + DeclareLaunchArgument('bag_replay_loop', default_value='false'), DeclareLaunchArgument('exposure_range_mode', default_value='default'),#default, ultimate or regular DeclareLaunchArgument('log_level', default_value='none'), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 9c5af9fd..6c859347 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -354,7 +354,7 @@ void OBCameraNode::setupDevices() { TRY_EXECUTE_BLOCK(device_->switchDepthWorkMode(depth_work_mode_.c_str())); RCLCPP_INFO_STREAM(logger_, "Set depth work mode: " << depth_work_mode_); } - if (!sync_mode_str_.empty()) { + if (!sync_mode_str_.empty() && is_physical_device(device_)) { auto sync_config = device_->getMultiDeviceSyncConfig(); RCLCPP_INFO_STREAM(logger_, "Current sync mode: " << magic_enum::enum_name(sync_config.syncMode)); @@ -1923,7 +1923,7 @@ void OBCameraNode::getParameters() { if (isOpenNIDevice(pid)) { time_domain_ = "system"; } - if (time_domain_ == "global") { + if (time_domain_ == "global" && is_physical_device(device_)) { device_->enableGlobalTimestamp(true); } setAndGetNodeParameter(frames_per_trigger_, "frames_per_trigger", 2); @@ -2093,7 +2093,7 @@ void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapp } void OBCameraNode::setupDiagnosticUpdater() { - if (diagnostic_period_ <= 0.0) { + if (diagnostic_period_ <= 0.0 || !is_physical_device(device_)) { return; } try { diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index 928e524b..d8f5df47 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -237,6 +237,8 @@ void OBCameraNodeDriver::init() { usb_port_ = declare_parameter("usb_port", ""); net_device_ip_ = declare_parameter("net_device_ip", ""); net_device_port_ = static_cast(declare_parameter("net_device_port", 0)); + bag_filename_ = declare_parameter("bag_filename", ""); + bag_replay_loop_ = static_cast(declare_parameter("bag_replay_loop", true)); enumerate_net_device_ = declare_parameter("enumerate_net_device", false); uvc_backend_ = declare_parameter("uvc_backend", "libuvc"); if (uvc_backend_ == "libuvc") { @@ -411,6 +413,8 @@ void OBCameraNodeDriver::queryDevice() { if (!enumerate_net_device_ && !net_device_ip_.empty() && net_device_port_ != 0) { connectNetDevice(net_device_ip_, net_device_port_); + } else if (!bag_filename_.empty()) { + openPlaybackDevice(bag_filename_, bag_replay_loop_); } else { auto device_list = ctx_->queryDeviceList(); if (device_list->getCount() != 0) { @@ -1135,6 +1139,51 @@ void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int } } +void OBCameraNodeDriver::openPlaybackDevice(const std::string &bagfile_path, bool bag_replay_loop) { + if (bagfile_path.empty() || !std::filesystem::exists(bagfile_path)) { + RCLCPP_ERROR_STREAM(logger_, "Invalid bag file path " << bagfile_path); + return; + } + + auto playback_device = std::make_shared(bagfile_path.c_str()); + if (playback_device == nullptr) { + RCLCPP_ERROR_STREAM(logger_, "Failed to open playback device " << bagfile_path); + return; + } + try { + RCLCPP_INFO_STREAM(logger_, "Opening bag file " << bagfile_path); + + // Automatically restart playback when reaching file end + playback_device->setPlaybackStatusChangeCallback([&](OBPlaybackStatus status) + { + RCLCPP_INFO(logger_, "Playback status changed to %d", status); + + if (status == OB_PLAYBACK_STOPPED) { + if (bag_replay_loop && is_alive_ && rclcpp::ok()) { + // restart bag file + ob_camera_node_->clean(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + initializeDevice(playback_device); + } else { + // stop it here + rclcpp::shutdown(); + } + } + }); + + initializeDevice(playback_device); + if (!device_connected_) { + RCLCPP_ERROR_STREAM(logger_, "Failed to initialize playback device " << bagfile_path); + } + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(logger_, "Exception during playback device initialization: " << e.what()); + device_connected_ = false; + } catch (...) { + RCLCPP_ERROR_STREAM(logger_, "Unknown exception during playback device initialization"); + device_connected_ = false; + } +} + void OBCameraNodeDriver::startDevice(const std::shared_ptr &list) { if (device_connected_.load()) { return; diff --git a/orbbec_camera/src/utils.cpp b/orbbec_camera/src/utils.cpp index e7945e0e..442d938e 100644 --- a/orbbec_camera/src/utils.cpp +++ b/orbbec_camera/src/utils.cpp @@ -953,4 +953,11 @@ std::string calcMD5(const std::string &data) { for (unsigned int i = 0; i < digest_len; ++i) ss << std::setw(2) << (int)digest[i]; return ss.str(); } + +bool is_physical_device(std::shared_ptr device) +{ + // any non PlaybackDevice is supposed to be a physical one + return (dynamic_cast(device.get()) == nullptr); +} + } // namespace orbbec_camera