i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-12 19:20:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: add bag record and playback support

Browse Source
This commit is contained in:
slz
2026-06-16 16:57:46 +08:00
parent 9126013267
commit d801d5670b
5 changed files with 115 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+7 -1
View File
@@ -164,7 +164,8 @@ typedef struct {
class OBCameraNode {
public:
OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device,
std::shared_ptr<Parameters> parameters, bool use_intra_process = false);
std::shared_ptr<Parameters> parameters, bool use_intra_process = false,
bool is_playback_device = false);
template <class T>
void setAndGetNodeParameter(
@@ -932,6 +933,11 @@ class OBCameraNode {
bool hw_d2c_color_undistortion_configured_ = false;
bool has_first_color_frame_ = false;
bool use_intra_process_ = false;
// True when device_ is an ob::PlaybackDevice. Real-device-only hardware
// tuning (heartbeat, USB3 retry, laser, sync config, etc.) is skipped in
// setupDevices() for playback devices since those SDK components aren't
// registered for a recorded .bag file.
bool is_playback_device_ = false;
std::string cloud_frame_id_;
std::mutex depth_filter_mutex_;
std::vector<std::shared_ptr<ob::Filter>> depth_filter_list_;
orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
+11
View File
@@ -31,6 +31,7 @@
#include <std_srvs/srv/empty.hpp>
#include <backward_ros/backward.hpp>
#include "libobsensor/hpp/Device.hpp"
#include "libobsensor/hpp/RecordPlayback.hpp"
namespace orbbec_camera {
@@ -57,6 +58,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
void initializeDevice(const std::shared_ptr<ob::Device>& device);
void initializeBagPlayback();
void startDevice(const std::shared_ptr<ob::DeviceList>& list);
void connectNetDevice(const std::string& net_device_ip, int net_device_port);
@@ -155,5 +158,13 @@ class OBCameraNodeDriver : public rclcpp::Node {
std::string force_ip_gateway_; // e.g. "192.168.1.1"
std::atomic<bool> force_ip_success_{false};
std::string device_type_;
// bag recording
std::string bag_record_filename_;
bool bag_record_compression_ = true;
std::shared_ptr<ob::RecordDevice> record_device_ = nullptr;
// bag playback
std::string bag_filename_;
bool bag_loop_ = false;
std::shared_ptr<ob::PlaybackDevice> playback_device_ = nullptr;
};
} // namespace orbbec_camera
orbbec_camera/launch/gemini_330_series.launch.py
+6
View File
@@ -75,6 +75,12 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
# Bag recording: record the live stream to an Orbbec .bag file
DeclareLaunchArgument('bag_record_filename', default_value=''),
DeclareLaunchArgument('bag_record_compression', default_value='true'),
# Bag playback: load a previously recorded .bag file as a virtual device
DeclareLaunchArgument('bag_filename', default_value=''),
DeclareLaunchArgument('bag_loop', default_value='false'),
DeclareLaunchArgument('upgrade_firmware', default_value=''),
DeclareLaunchArgument('preset_firmware_path', default_value=''),
DeclareLaunchArgument('load_config_json_file_path', default_value=''),
orbbec_camera/src/ob_camera_node.cpp
+17 -3
View File
@@ -548,12 +548,14 @@ void OBCameraNode::publishDepthFiltersStatus() {
}
OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
std::shared_ptr<Parameters> parameters, bool use_intra_process)
std::shared_ptr<Parameters> parameters, bool use_intra_process,
bool is_playback_device)
: node_(node),
device_(std::move(device)),
parameters_(std::move(parameters)),
logger_(node->get_logger()),
use_intra_process_(use_intra_process) {
use_intra_process_(use_intra_process),
is_playback_device_(is_playback_device) {
pid_ = device_->getDeviceInfo()->getPid();
RCLCPP_INFO_STREAM(logger_,
"OBCameraNode: use_intra_process: " << (use_intra_process ? "ON" : "OFF"));
@@ -879,6 +881,16 @@ void OBCameraNode::setupDevices() {
}
auto device_info = device_->getDeviceInfo();
CHECK_NOTNULL(device_info);
if (is_playback_device_) {
// Everything below this point only tunes real-hardware-only behavior
// (heartbeat, USB3 retry, laser, depth limits, multi-device sync, PTP,
// etc.). Those SDK device components aren't registered on a playback
// device, so querying them throws instead of returning "not supported".
// None of it is needed to correctly publish a recorded .bag file.
return;
}
auto should_apply_launch_config = [this](const std::string &param_name) {
return isLaunchParamProvided(param_name);
};
@@ -3967,7 +3979,9 @@ void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapp
}
void OBCameraNode::setupDiagnosticUpdater() {
if (diagnostic_period_ <= 0.0) {
if (diagnostic_period_ <= 0.0 || is_playback_device_) {
// Temperature reporting requires live hardware monitoring that doesn't
// exist for a recorded .bag file; skip to avoid repeated SDK errors.
return;
}
try {
orbbec_camera/src/ob_camera_node_driver.cpp
+74 -3
View File
@@ -161,6 +161,13 @@ OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::
OBCameraNodeDriver::~OBCameraNodeDriver() {
is_alive_.store(false);
// Finalize bag recording before the pipeline is torn down, otherwise the
// bag file can end up truncated/corrupted.
if (record_device_) {
RCLCPP_INFO_STREAM(logger_, "Finalizing bag recording...");
record_device_.reset();
}
// First stop the camera node cleanly before stopping threads
if (ob_camera_node_) {
try {
@@ -267,6 +274,19 @@ void OBCameraNodeDriver::init() {
RCLCPP_WARN_STREAM(
logger_, "Failed to set SDK log file name: " << orbbec_camera::formatObErrorWithStatus(e));
}
// Bag file playback mode: load a previously recorded .bag file as a virtual
// device instead of enumerating real hardware. Must be checked before the
// ob::Context / device discovery machinery is set up below.
device_type_ = declare_parameter<std::string>("device_type", "camera");
bag_filename_ = declare_parameter<std::string>("bag_filename", "");
bag_loop_ = declare_parameter<bool>("bag_loop", false);
if (!bag_filename_.empty()) {
is_alive_.store(true);
parameters_ = std::make_shared<Parameters>(this);
initializeBagPlayback();
return;
}
// Force IP
force_ip_enable_ = declare_parameter<bool>("force_ip_enable", false);
force_ip_mac_ = declare_parameter<std::string>("force_ip_mac", "");
@@ -293,7 +313,6 @@ void OBCameraNodeDriver::init() {
}
applyForceIpConfig();
device_type_ = declare_parameter<std::string>("device_type", "camera");
connection_delay_ = static_cast<int>(declare_parameter<int>("connection_delay", 100));
enable_sync_host_time_ = declare_parameter<bool>("enable_sync_host_time", true);
double time_sync_period = declare_parameter<double>("time_sync_period", 60.0);
@@ -331,6 +350,8 @@ void OBCameraNodeDriver::init() {
last_reset_device_completion_time_ = std::chrono::steady_clock::now() - std::chrono::seconds(10);
parameters_ = std::make_shared<Parameters>(this);
serial_number_ = declare_parameter<std::string>("serial_number", "");
bag_record_filename_ = declare_parameter<std::string>("bag_record_filename", "");
bag_record_compression_ = declare_parameter<bool>("bag_record_compression", true);
device_num_ = static_cast<int>(declare_parameter<int>("device_num", 1));
usb_port_ = declare_parameter<std::string>("usb_port", "");
net_device_ip_ = declare_parameter<std::string>("net_device_ip", "");
@@ -562,6 +583,12 @@ void OBCameraNodeDriver::resetDevice() {
device_connected_ = false;
device_connecting_ = false; // Clear connecting flag
// Stop recording before tearing down the pipeline so the bag file is finalized
if (record_device_) {
RCLCPP_WARN_STREAM(logger_, "Device disconnected, stopping bag recording");
record_device_.reset();
}
// Reset objects in order, with additional safety checks
if (ob_camera_node_) {
ob_camera_node_.reset();
@@ -979,6 +1006,37 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
return nullptr;
}
void OBCameraNodeDriver::initializeBagPlayback() {
RCLCPP_INFO_STREAM(logger_, "Starting bag file playback: " << bag_filename_);
try {
playback_device_ = std::make_shared<ob::PlaybackDevice>(bag_filename_);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to open bag file: " << orbbec_camera::formatObErrorWithStatus(e));
return;
}
if (bag_loop_) {
playback_device_->setPlaybackStatusChangeCallback([this](OBPlaybackStatus status) {
if (status == OB_PLAYBACK_STOPPED && is_alive_) {
RCLCPP_INFO_STREAM(logger_, "Bag playback completed, restarting from beginning...");
try {
playback_device_->seek(0);
if (ob_camera_node_) {
ob_camera_node_->startStreams();
}
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "Failed to restart bag playback: "
<< orbbec_camera::formatObErrorWithStatus(e));
}
}
});
}
std::shared_ptr<ob::Device> device = playback_device_;
initializeDevice(device);
}
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
device_ = device;
updatePresetFirmware(preset_firmware_path_);
@@ -999,7 +1057,8 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
try {
if (device_type_ == "camera") {
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
node_options_.use_intra_process_comms());
node_options_.use_intra_process_comms(),
playback_device_ != nullptr);
} else if (device_type_ == "lidar") {
ob_lidar_node_ = std::make_unique<orbbec_lidar::OBLidarNode>(
this, device_, parameters_, node_options_.use_intra_process_comms());
@@ -1034,7 +1093,8 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
CHECK_NOTNULL(device_info_.get());
device_unique_id_ = device_info_->getUid();
if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid()) && device_type_ == "camera") {
if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid()) && device_type_ == "camera" &&
!playback_device_) {
TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
if (g_time_domain != "global") {
device_->enableGlobalTimestamp(false);
@@ -1183,6 +1243,17 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
RCLCPP_WARN_STREAM(logger_, "Camera or LiDAR node is null after device initialization");
}
if (!bag_record_filename_.empty() && !record_device_) {
try {
record_device_ = std::make_shared<ob::RecordDevice>(device_, bag_record_filename_,
bag_record_compression_);
RCLCPP_INFO_STREAM(logger_, "Recording to bag file: " << bag_record_filename_);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(
logger_, "Failed to start recording: " << orbbec_camera::formatObErrorWithStatus(e));
}
}
} // namespace orbbec_camera
bool OBCameraNodeDriver::applyForceIpConfig() {