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

Merge remote-tracking branch 'origin/feat/bag-record-playback' into merge/sdk_2.9.0

Browse Source
This commit is contained in:
ob-yalian
2026-06-20 16:24:42 +08:00
parent 0384847615 f8f37a34fd
commit 7c88f3c581
26 changed files with 317 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+9 -1
View File
@@ -166,7 +166,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(
@@ -193,6 +194,8 @@ class OBCameraNode {
void startIMU();
void restartPlaybackStreams();
int openSocSyncPwmTrigger(uint16_t fps);
int closeSocSyncPwmTrigger();
void startGmslTrigger();
@@ -949,6 +952,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
+20
View File
@@ -31,6 +31,8 @@
#include <std_srvs/srv/empty.hpp>
#include <backward_ros/backward.hpp>
#include "libobsensor/hpp/Device.hpp"
#include "libobsensor/hpp/RecordPlayback.hpp"
#include "orbbec_camera_msgs/srv/set_bag_recording.hpp"
namespace orbbec_camera {
@@ -57,6 +59,10 @@ class OBCameraNodeDriver : public rclcpp::Node {
void initializeDevice(const std::shared_ptr<ob::Device>& device);
void initializeBagPlayback();
void exportBagPresetJson(const std::string& bag_path);
void startDevice(const std::shared_ptr<ob::DeviceList>& list);
void connectNetDevice(const std::string& net_device_ip, int net_device_port);
@@ -77,6 +83,10 @@ class OBCameraNodeDriver : public rclcpp::Node {
void rebootDeviceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response);
void setBagRecordingCallback(
const std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Response> response);
void presetUpdateCallback(bool firstCall, OBFwUpdateState state, const char* message,
uint8_t percent);
void updatePresetFirmware(std::string path);
@@ -136,6 +146,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
std::string timestamp_clock_type_str_;
std::string preset_firmware_path_;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
rclcpp::Service<orbbec_camera_msgs::srv::SetBagRecording>::SharedPtr set_bag_recording_srv_ =
nullptr;
std::chrono::time_point<std::chrono::system_clock> start_time_;
std::string extension_path_;
static backward::SignalHandling sh; // for stack trace
@@ -155,5 +167,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/astra.launch.py
+3
View File
@@ -17,6 +17,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
DeclareLaunchArgument('bag_filename', default_value=''),
DeclareLaunchArgument('bag_loop', default_value='false'),
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
orbbec_camera/launch/astra2.launch.py
+3
View File
@@ -17,6 +17,9 @@ def generate_launch_description():
DeclareLaunchArgument("serial_number", default_value=""),
DeclareLaunchArgument("usb_port", default_value=""),
DeclareLaunchArgument("device_num", default_value="1"),
DeclareLaunchArgument("bag_record_filename", default_value=""),
DeclareLaunchArgument("bag_filename", default_value=""),
DeclareLaunchArgument("bag_loop", default_value="false"),
DeclareLaunchArgument("uvc_backend", default_value="libuvc"), # libuvc or v4l2
DeclareLaunchArgument("product_id", default_value=""),
DeclareLaunchArgument("enable_point_cloud", default_value="true"),
orbbec_camera/launch/dabai_a.launch.py
+4 -1
View File
@@ -42,7 +42,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
result = {}
for key, value in default_params.items():
@@ -74,6 +74,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
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/launch/dabai_al.launch.py
+4 -1
View File
@@ -42,7 +42,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
result = {}
for key, value in default_params.items():
@@ -74,6 +74,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
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/launch/dabai_dcw2.launch.py
+3
View File
@@ -17,6 +17,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
DeclareLaunchArgument('bag_filename', default_value=''),
DeclareLaunchArgument('bag_loop', default_value='false'),
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
orbbec_camera/launch/dabai_max_pro.launch.py
+3
View File
@@ -17,6 +17,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
DeclareLaunchArgument('bag_filename', default_value=''),
DeclareLaunchArgument('bag_loop', default_value='false'),
DeclareLaunchArgument('vendor_id', default_value='0x2bc5'),
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
orbbec_camera/launch/femto.launch.py
+3
View File
@@ -17,6 +17,9 @@ def generate_launch_description():
DeclareLaunchArgument("serial_number", default_value=""),
DeclareLaunchArgument("usb_port", default_value=""),
DeclareLaunchArgument("device_num", default_value="1"),
DeclareLaunchArgument("bag_record_filename", default_value=""),
DeclareLaunchArgument("bag_filename", default_value=""),
DeclareLaunchArgument("bag_loop", default_value="false"),
DeclareLaunchArgument("product_id", default_value=""),
DeclareLaunchArgument("enable_point_cloud", default_value="true"),
DeclareLaunchArgument("cloud_frame_id", default_value=""),
orbbec_camera/launch/femto_bolt.launch.py
+3
View File
@@ -17,6 +17,9 @@ def generate_launch_description():
DeclareLaunchArgument("serial_number", default_value=""),
DeclareLaunchArgument("usb_port", default_value=""),
DeclareLaunchArgument("device_num", default_value="1"),
DeclareLaunchArgument("bag_record_filename", default_value=""),
DeclareLaunchArgument("bag_filename", default_value=""),
DeclareLaunchArgument("bag_loop", default_value="false"),
DeclareLaunchArgument("uvc_backend", default_value="libuvc"), # libuvc or v4l2
DeclareLaunchArgument("product_id", default_value=""),
DeclareLaunchArgument("enable_point_cloud", default_value="false"),
orbbec_camera/launch/femto_mega.launch.py
+3
View File
@@ -17,6 +17,9 @@ def generate_launch_description():
DeclareLaunchArgument("serial_number", default_value=""),
DeclareLaunchArgument("usb_port", default_value=""),
DeclareLaunchArgument("device_num", default_value="1"),
DeclareLaunchArgument("bag_record_filename", default_value=""),
DeclareLaunchArgument("bag_filename", default_value=""),
DeclareLaunchArgument("bag_loop", default_value="false"),
DeclareLaunchArgument("uvc_backend", default_value="libuvc"), # libuvc or v4l2
DeclareLaunchArgument("product_id", default_value=""),
DeclareLaunchArgument("enable_point_cloud", default_value="true"),
orbbec_camera/launch/gemini2.launch.py
+3
View File
@@ -17,6 +17,9 @@ def generate_launch_description():
DeclareLaunchArgument("serial_number", default_value=""),
DeclareLaunchArgument("usb_port", default_value=""),
DeclareLaunchArgument("device_num", default_value="1"),
DeclareLaunchArgument("bag_record_filename", default_value=""),
DeclareLaunchArgument("bag_filename", default_value=""),
DeclareLaunchArgument("bag_loop", default_value="false"),
DeclareLaunchArgument("uvc_backend", default_value="libuvc"), # libuvc or v4l2
DeclareLaunchArgument("product_id", default_value=""),
DeclareLaunchArgument("enable_point_cloud", default_value="true"),
orbbec_camera/launch/gemini210.launch.py
+3
View File
@@ -17,6 +17,9 @@ def generate_launch_description():
DeclareLaunchArgument("serial_number", default_value=""),
DeclareLaunchArgument("usb_port", default_value=""),
DeclareLaunchArgument("device_num", default_value="1"),
DeclareLaunchArgument("bag_record_filename", default_value=""),
DeclareLaunchArgument("bag_filename", default_value=""),
DeclareLaunchArgument("bag_loop", default_value="false"),
DeclareLaunchArgument("uvc_backend", default_value="libuvc"), # libuvc or v4l2
DeclareLaunchArgument("product_id", default_value=""),
DeclareLaunchArgument("enable_point_cloud", default_value="true"),
orbbec_camera/launch/gemini2L.launch.py
+4 -1
View File
@@ -64,7 +64,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {"config_file_path", "usb_port", "serial_number"}
skip_convert = {"config_file_path", "usb_port", "serial_number", 'bag_record_filename', 'bag_filename'}
return {
key: (value if key in skip_convert else convert_value(value))
@@ -79,6 +79,9 @@ def generate_launch_description():
DeclareLaunchArgument("serial_number", default_value=""),
DeclareLaunchArgument("usb_port", default_value=""),
DeclareLaunchArgument("device_num", default_value="1"),
DeclareLaunchArgument("bag_record_filename", default_value=""),
DeclareLaunchArgument("bag_filename", default_value=""),
DeclareLaunchArgument("bag_loop", default_value="false"),
DeclareLaunchArgument("uvc_backend", default_value="libuvc"), # libuvc or v4l2
DeclareLaunchArgument("product_id", default_value=""),
DeclareLaunchArgument("enable_point_cloud", default_value="true"),
orbbec_camera/launch/gemini345.launch.py
+4 -1
View File
@@ -42,7 +42,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
result = {}
for key, value in default_params.items():
@@ -73,6 +73,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
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/launch/gemini345_lg.launch.py
+4 -1
View File
@@ -42,7 +42,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
result = {}
for key, value in default_params.items():
@@ -74,6 +74,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
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/launch/gemini435_le.launch.py
+4 -1
View File
@@ -42,7 +42,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
result = {}
for key, value in default_params.items():
@@ -74,6 +74,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
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/launch/gemini_301_series.launch.py
+4 -1
View File
@@ -64,7 +64,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
result = {}
for key, value in default_params.items():
@@ -97,6 +97,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
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/launch/gemini_330_series.launch.py
+6 -1
View File
@@ -42,7 +42,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
result = {}
for key, value in default_params.items():
@@ -75,6 +75,11 @@ 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=''),
# 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/launch/gemini_330_series_low_cpu.launch.py
+4 -1
View File
@@ -42,7 +42,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
result = {}
for key, value in default_params.items():
@@ -75,6 +75,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
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/launch/gemini_330_series_sdk_json.launch.py
+4 -1
View File
@@ -41,7 +41,7 @@ def load_parameters(context, args):
config_file_path = LaunchConfiguration('config_file_path').perform(context)
if config_file_path:
default_params = merge_params(default_params, load_yaml(config_file_path))
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
result = {}
for key, value in default_params.items():
@@ -72,6 +72,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
DeclareLaunchArgument('bag_filename', default_value=''),
DeclareLaunchArgument('bag_loop', default_value='false'),
DeclareLaunchArgument('connection_delay', default_value='10'),
DeclareLaunchArgument('upgrade_firmware', default_value=''),
DeclareLaunchArgument('preset_firmware_path', default_value=''),
orbbec_camera/launch/gemini_intra_process_demo_launch.py
+4 -1
View File
@@ -42,7 +42,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
return {
key: (value if key in skip_convert else convert_value(value))
for key, value in default_params.items()
@@ -56,6 +56,9 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('bag_record_filename', default_value=''),
DeclareLaunchArgument('bag_filename', default_value=''),
DeclareLaunchArgument('bag_loop', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('enable_point_cloud', default_value='false'),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='true'),
orbbec_camera/src/ob_camera_node.cpp
+38 -4
View File
@@ -572,12 +572,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"));
@@ -903,6 +905,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);
};
@@ -3613,6 +3625,26 @@ void OBCameraNode::startIMU() {
}
}
void OBCameraNode::restartPlaybackStreams() {
if (!is_playback_device_ || !is_running_.load()) {
return;
}
try {
stopStreams();
startStreams();
stopIMU();
startIMU();
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "Failed to restart playback streams: "
<< orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_WARN_STREAM(logger_, "Failed to restart playback streams: " << e.what());
} catch (...) {
RCLCPP_WARN_STREAM(logger_, "Failed to restart playback streams");
}
}
void OBCameraNode::stopStreams() {
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
@@ -4099,7 +4131,7 @@ void OBCameraNode::getParameters() {
if (isOpenNIDevice(pid_)) {
time_domain_ = "system";
}
if (time_domain_ == "global") {
if (time_domain_ == "global" && !is_playback_device_) {
device_->enableGlobalTimestamp(true);
}
setAndGetNodeParameter<int>(frames_per_trigger_, "frames_per_trigger", 2);
@@ -4291,7 +4323,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
+171 -3
View File
@@ -32,6 +32,7 @@
#include <fstream>
#include <iomanip> // For std::put_time
#include <malloc.h>
#include <sstream>
std::string g_camera_name = "orbbec_camera"; // Assuming this is declared elsewhere
std::string g_time_domain = "global"; // Assuming this is declared elsewhere
@@ -45,6 +46,18 @@ std::string getLogDirectoryForCamera(const std::string &camera_name) {
return (std::filesystem::path(home_dir) / ".ros" / "Log" / camera_name).string();
}
std::string getDefaultBagRecordFilePath() {
const std::time_t now_time = std::time(nullptr);
std::tm tm{};
localtime_r(&now_time, &tm);
std::ostringstream time_stream;
time_stream << std::put_time(&tm, "%Y_%m_%d_%H_%M_%S");
return (std::filesystem::current_path() / ("orbbec_record_" + time_stream.str() + ".bag"))
.string();
}
std::string makeDefaultSdkLogFileName() {
const std::time_t now_time = std::time(nullptr);
std::tm tm{};
@@ -161,6 +174,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_) {
record_device_.reset();
RCLCPP_INFO_STREAM(logger_, "Bag recording stopped");
}
// First stop the camera node cleanly before stopping threads
if (ob_camera_node_) {
try {
@@ -267,6 +287,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 +326,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);
@@ -322,6 +354,9 @@ void OBCameraNodeDriver::init() {
reboot_device_srv_ = this->create_service<std_srvs::srv::Empty>(
"reboot_device", std::bind(&OBCameraNodeDriver::rebootDeviceCallback, this,
std::placeholders::_1, std::placeholders::_2));
set_bag_recording_srv_ = this->create_service<orbbec_camera_msgs::srv::SetBagRecording>(
"set_bag_recording", std::bind(&OBCameraNodeDriver::setBagRecordingCallback, this,
std::placeholders::_1, std::placeholders::_2));
pthread_mutexattr_init(&orb_device_lock_attr_);
pthread_mutexattr_setpshared(&orb_device_lock_attr_, PTHREAD_PROCESS_SHARED);
orb_device_lock_ = (pthread_mutex_t *)orb_device_lock_shm_addr_;
@@ -331,6 +366,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 +599,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_) {
record_device_.reset();
RCLCPP_WARN_STREAM(logger_, "Device disconnected, bag recording stopped");
}
// Reset objects in order, with additional safety checks
if (ob_camera_node_) {
ob_camera_node_.reset();
@@ -778,6 +821,56 @@ void OBCameraNodeDriver::deviceStatusTimer() {
}
// RCLCPP_INFO_STREAM(logger_, "deviceStatusTimer() ");
}
void OBCameraNodeDriver::setBagRecordingCallback(
const std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Response> response) {
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
if (!device_) {
response->success = false;
response->message = "No device connected";
return;
}
if (!request->enable) {
if (!record_device_) {
response->success = true;
response->message = "Bag recording is not running";
return;
}
record_device_.reset();
RCLCPP_INFO_STREAM(logger_, "Bag recording stopped");
response->success = true;
response->message = "Bag recording stopped";
return;
}
std::string file_path =
request->file_path.empty() ? getDefaultBagRecordFilePath() : request->file_path;
if (record_device_) {
record_device_.reset();
RCLCPP_INFO_STREAM(logger_, "Bag recording stopped before starting a new recording");
}
try {
exportBagPresetJson(file_path);
record_device_ =
std::make_shared<ob::RecordDevice>(device_, file_path, bag_record_compression_);
} catch (const ob::Error &e) {
response->success = false;
response->message = "Failed to start recording: " + orbbec_camera::formatObErrorWithStatus(e);
RCLCPP_ERROR_STREAM(logger_, response->message);
return;
}
RCLCPP_INFO_STREAM(logger_, "Recording to " << file_path);
response->success = true;
response->message = "Recording started: " + file_path;
}
void OBCameraNodeDriver::rebootDeviceCallback(
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response) {
@@ -979,6 +1072,67 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
return nullptr;
}
void OBCameraNodeDriver::exportBagPresetJson(const std::string &bag_path) {
if (!device_ || bag_path.empty() || playback_device_) {
return;
}
auto json_path = std::filesystem::path(bag_path);
if (json_path.extension() == ".bag") {
json_path.replace_extension(".json");
} else {
json_path += ".json";
}
const auto json_path_str = json_path.string();
try {
const auto parent_path = json_path.parent_path();
if (!parent_path.empty()) {
std::filesystem::create_directories(parent_path);
}
device_->exportSettingsAsPresetJsonFile(json_path_str.c_str());
RCLCPP_INFO_STREAM(logger_, "Exported bag preset JSON: " << json_path_str);
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "Failed to export bag preset JSON "
<< json_path_str << ": "
<< orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_WARN_STREAM(logger_,
"Failed to export bag preset JSON " << json_path_str << ": " << e.what());
}
}
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_->restartPlaybackStreams();
}
} 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 +1153,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 +1189,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 +1339,18 @@ 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 {
exportBagPresetJson(bag_record_filename_);
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() {
orbbec_camera_msgs/CMakeLists.txt
+1
View File
@@ -37,6 +37,7 @@ rosidl_generate_interfaces(
"srv/SetArrays.srv"
"srv/GetUserCalibParams.srv"
"srv/SetUserCalibParams.srv"
"srv/SetBagRecording.srv"
DEPENDENCIES
sensor_msgs
std_msgs
orbbec_camera_msgs/srv/SetBagRecording.srv
+5
View File
@@ -0,0 +1,5 @@
bool enable # true starts recording, false stops recording
string file_path # path to save the .bag file when enable is true; empty uses current directory
---
bool success
string message