From 8d36dedaee34ee466b734c7d652ce06f31902c10 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Fri, 24 Oct 2025 15:52:20 -0400 Subject: [PATCH 01/33] [RSDK-12362] Provide Support for Orbbec Gemini 335LE Camera --- src/module/discovery.cpp | 82 +++++++++++++++----- src/module/orbbec.cpp | 156 ++++++++++++++++++++++++++------------- 2 files changed, 169 insertions(+), 69 deletions(-) diff --git a/src/module/discovery.cpp b/src/module/discovery.cpp index 8576ae82..1032f9c7 100644 --- a/src/module/discovery.cpp +++ b/src/module/discovery.cpp @@ -12,35 +12,81 @@ OrbbecDiscovery::OrbbecDiscovery(vsdk::Dependencies dependencies, vsdk::Resource std::vector OrbbecDiscovery::discover_resources(const vsdk::ProtoStruct& extra) { std::vector configs; - std::shared_ptr devList = ob_ctx_->queryDeviceList(); - int devCount = devList->getCount(); - - if (devCount == 0) { - VIAM_SDK_LOG(warn) << "No Orbbec devices found during discovery"; - return {}; + // Enable network device enumeration for Ethernet cameras (like Gemini 335Le) + try { + ob_ctx_->enableNetDeviceEnumeration(true); + VIAM_RESOURCE_LOG(info) << "Enabled network device enumeration for Ethernet cameras"; + } catch (ob::Error& e) { + VIAM_RESOURCE_LOG(warn) << "Failed to enable network device enumeration: " << e.what(); + } catch (const std::exception& e) { + VIAM_RESOURCE_LOG(warn) << "Failed to enable network device enumeration: " << e.what(); } - VIAM_SDK_LOG(info) << "Discovered " << devCount << " devices"; + try { + // Use SDK's native device discovery (works for both USB and network devices) + std::shared_ptr devList = ob_ctx_->queryDeviceList(); + int devCount = devList->getCount(); + + if (devCount == 0) { + VIAM_RESOURCE_LOG(warn) << "No Orbbec devices found during discovery"; + return {}; + } + + VIAM_RESOURCE_LOG(info) << "Discovered " << devCount << " Orbbec devices"; + + for (size_t i = 0; i < devCount; i++) { + try { + std::string deviceName = devList->name(i); + std::string serialNumber = devList->serialNumber(i); + std::string connectionType = devList->connectionType(i); + std::string ipAddress = devList->ipAddress(i); - for (size_t i = 0; i < devCount; i++) { - std::shared_ptr dev = devList->getDevice(i); - std::shared_ptr info = dev->getDeviceInfo(); + std::stringstream deviceInfoString; + deviceInfoString << "Device " << (i + 1) << " - Name: " << deviceName + << ", Serial: " << serialNumber + << ", Connection: " << connectionType; + if(!ipAddress.empty()) { + deviceInfoString << ", IP: " << ipAddress; + } + + VIAM_RESOURCE_LOG(info) << deviceInfoString.str(); - std::ostringstream name; - name << "orbbec-" << i + 1; + std::ostringstream name; + name << "orbbec-" << i + 1; - vsdk::ProtoStruct attributes; - attributes.emplace("serial_number", info->serialNumber()); + vsdk::ProtoStruct attributes; + attributes.emplace("serial_number", serialNumber); - vsdk::ResourceConfig config( - "camera", std::move(name.str()), "viam", attributes, "rdk:component:camera", orbbec::Orbbec::model, vsdk::log_level::info); - configs.push_back(config); + vsdk::ResourceConfig config( + "camera", std::move(name.str()), "viam", attributes, "rdk:component:camera", orbbec::Orbbec::model, vsdk::log_level::info); + configs.push_back(config); + + VIAM_RESOURCE_LOG(info) << "Successfully configured device " << (i + 1) << " with serial: " << serialNumber; + } catch (ob::Error& deviceError) { + VIAM_RESOURCE_LOG(warn) << "Failed to get device info for device " << (i + 1) << ": " << deviceError.what(); + // Continue with other devices even if one fails + } + } + } catch (ob::Error& e) { + VIAM_RESOURCE_LOG(error) << "Failed to discover Orbbec devices: " << e.what() + << " (function: " << e.getFunction() + << ", args: " << e.getArgs() + << ", name: " << e.getName() + << ", type: " << e.getExceptionType() << ")"; + VIAM_RESOURCE_LOG(warn) << "Discovery failed - check network connectivity for Ethernet cameras or USB connection for USB cameras"; + } catch (const std::exception& e) { + VIAM_RESOURCE_LOG(error) << "Failed to discover Orbbec devices: " << e.what(); + VIAM_RESOURCE_LOG(warn) << "Discovery failed - check network connectivity for Ethernet cameras or USB connection for USB cameras"; + } catch (...) { + VIAM_RESOURCE_LOG(error) << "Failed to discover Orbbec devices: unknown error"; + VIAM_RESOURCE_LOG(warn) << "Discovery failed - check network connectivity for Ethernet cameras or USB connection for USB cameras"; } + return configs; } vsdk::ProtoStruct OrbbecDiscovery::do_command(const vsdk::ProtoStruct& command) { - VIAM_SDK_LOG(error) << "do_command not implemented"; + VIAM_RESOURCE_LOG(error) << "do_command not implemented"; return vsdk::ProtoStruct{}; } diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index f3577880..568c76d6 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -867,12 +867,12 @@ void applyExperimentalConfig(std::unique_ptr& my_dev, vsdk::ProtoS Orbbec::Orbbec(vsdk::Dependencies deps, vsdk::ResourceConfig cfg, std::shared_ptr ctx) : Camera(cfg.name()), serial_number_(getSerialNumber(cfg)), ob_ctx_(std::move(ctx)) { - VIAM_SDK_LOG(info) << "Orbbec constructor start " << serial_number_; + VIAM_RESOURCE_LOG(info) << "Orbbec constructor start " << serial_number_; auto config = configure(deps, cfg); { std::lock_guard lock(config_by_serial_mu()); config_by_serial().insert_or_assign(serial_number_, *config); - VIAM_SDK_LOG(info) << "initial config_by_serial_: " << config_by_serial().at(serial_number_).to_string(); + VIAM_RESOURCE_LOG(info) << "initial config_by_serial_: " << config_by_serial().at(serial_number_).to_string(); } { @@ -902,17 +902,17 @@ Orbbec::Orbbec(vsdk::Dependencies deps, vsdk::ResourceConfig cfg, std::shared_pt } } - VIAM_SDK_LOG(info) << "Orbbec constructor end " << serial_number_; + VIAM_RESOURCE_LOG(info) << "Orbbec constructor end " << serial_number_; } Orbbec::~Orbbec() { - VIAM_SDK_LOG(info) << "Orbbec destructor start " << serial_number_; + VIAM_RESOURCE_LOG(info) << "Orbbec destructor start " << serial_number_; std::string prev_serial_number; std::string prev_resource_name; { const std::lock_guard lock(config_by_serial_mu()); if (config_by_serial().count(serial_number_) == 0) { - VIAM_SDK_LOG(error) << "Orbbec destructor: device with serial number " << serial_number_ + VIAM_RESOURCE_LOG(error) << "Orbbec destructor: device with serial number " << serial_number_ << " is not in config_by_serial, skipping erase"; } else { prev_serial_number = config_by_serial().at(serial_number_).serial_number; @@ -920,11 +920,11 @@ Orbbec::~Orbbec() { } } stopDevice(prev_serial_number, prev_resource_name); - VIAM_SDK_LOG(info) << "Orbbec destructor end " << serial_number_; + VIAM_RESOURCE_LOG(info) << "Orbbec destructor end " << serial_number_; } void Orbbec::reconfigure(const vsdk::Dependencies& deps, const vsdk::ResourceConfig& cfg) { - VIAM_SDK_LOG(info) << "[reconfigure] Orbbec reconfigure start"; + VIAM_RESOURCE_LOG(info) << "[reconfigure] Orbbec reconfigure start"; std::string prev_serial_number; std::string prev_resource_name; { @@ -933,7 +933,7 @@ void Orbbec::reconfigure(const vsdk::Dependencies& deps, const vsdk::ResourceCon if (config_by_serial().count(serial_number_) == 0) { std::ostringstream buffer; buffer << "[reconfigure] device with serial number " << serial_number_ << " is not in config_by_serial, skipping reconfigure"; - VIAM_SDK_LOG(error) << buffer.str(); + VIAM_RESOURCE_LOG(error) << buffer.str(); throw std::runtime_error(buffer.str()); } else { prev_serial_number = config_by_serial().at(serial_number_).serial_number; @@ -956,7 +956,7 @@ void Orbbec::reconfigure(const vsdk::Dependencies& deps, const vsdk::ResourceCon } new_serial_number = config->serial_number; new_resource_name = config->resource_name; - VIAM_SDK_LOG(info) << "[reconfigure] updated config_by_serial_: " << config_by_serial().at(new_serial_number).to_string(); + VIAM_RESOURCE_LOG(info) << "[reconfigure] updated config_by_serial_: " << config_by_serial().at(new_serial_number).to_string(); } { @@ -981,12 +981,12 @@ void Orbbec::reconfigure(const vsdk::Dependencies& deps, const vsdk::ResourceCon applyExperimentalConfig(my_dev, cfg.attributes()); } } - VIAM_SDK_LOG(info) << "[reconfigure] Orbbec reconfigure end"; + VIAM_RESOURCE_LOG(info) << "[reconfigure] Orbbec reconfigure end"; } vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::ProtoStruct& extra) { try { - VIAM_SDK_LOG(debug) << "[get_image] start"; + VIAM_RESOURCE_LOG(debug) << "[get_image] start"; std::string serial_number; { const std::lock_guard lock(serial_number_mu_); @@ -1016,7 +1016,7 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro for (const auto& fmt : supported_color_formats) { buffer << fmt << " "; } - VIAM_SDK_LOG(error) << buffer.str(); + VIAM_RESOURCE_LOG(error) << buffer.str(); throw std::invalid_argument(buffer.str()); } @@ -1035,14 +1035,14 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro std::ostringstream buffer; buffer << "color frame format " << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()) << " does not match configured color format " << res_format_opt->color_format.value(); - VIAM_SDK_LOG(error) << buffer.str(); + VIAM_RESOURCE_LOG(error) << buffer.str(); throw std::invalid_argument(buffer.str()); } } else if (ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()) != Orbbec::default_color_format) { std::ostringstream buffer; buffer << "color frame format " << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()) << " does not match default color format " << Orbbec::default_color_format; - VIAM_SDK_LOG(error) << buffer.str(); + VIAM_RESOURCE_LOG(error) << buffer.str(); throw std::invalid_argument(buffer.str()); } @@ -1076,20 +1076,20 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro } else { std::ostringstream buffer; buffer << "[get_image] unsupported color format: " << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()); - VIAM_SDK_LOG(error) << buffer.str(); + VIAM_RESOURCE_LOG(error) << buffer.str(); throw std::invalid_argument(buffer.str()); } - VIAM_SDK_LOG(debug) << "[get_image] end"; + VIAM_RESOURCE_LOG(debug) << "[get_image] end"; return response; } catch (const std::exception& e) { - VIAM_SDK_LOG(error) << "[get_image] error: " << e.what(); + VIAM_RESOURCE_LOG(error) << "[get_image] error: " << e.what(); throw std::runtime_error("failed to create image: " + std::string(e.what())); } } vsdk::Camera::properties Orbbec::get_properties() { try { - VIAM_SDK_LOG(debug) << "[get_properties] start"; + VIAM_RESOURCE_LOG(debug) << "[get_properties] start"; std::string serial_number; { @@ -1165,17 +1165,17 @@ vsdk::Camera::properties Orbbec::get_properties() { distortion_props.k5, distortion_props.k6}; - VIAM_SDK_LOG(debug) << "[get_properties] end"; + VIAM_RESOURCE_LOG(debug) << "[get_properties] end"; return p; } catch (const std::exception& e) { - VIAM_SDK_LOG(error) << "[get_properties] error: " << e.what(); + VIAM_RESOURCE_LOG(error) << "[get_properties] error: " << e.what(); throw std::runtime_error("failed to create properties: " + std::string(e.what())); } } vsdk::Camera::image_collection Orbbec::get_images(std::vector filter_source_names, const vsdk::ProtoStruct& extra) { try { - VIAM_SDK_LOG(debug) << "[get_images] start"; + VIAM_RESOURCE_LOG(debug) << "[get_images] start"; std::string serial_number; { const std::lock_guard lock(serial_number_mu_); @@ -1240,7 +1240,7 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte buffer << format << " "; } - VIAM_SDK_LOG(error) << buffer.str(); + VIAM_RESOURCE_LOG(error) << buffer.str(); throw std::runtime_error(buffer.str()); } @@ -1290,7 +1290,7 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte } else { std::ostringstream buffer; buffer << "[get_images] unsupported color format: " << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()); - VIAM_SDK_LOG(error) << buffer.str(); + VIAM_RESOURCE_LOG(error) << buffer.str(); throw std::runtime_error(buffer.str()); } } @@ -1307,7 +1307,7 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte for (const auto& format : supported_depth_formats) { buffer << format << " "; } - VIAM_SDK_LOG(error) << buffer.str(); + VIAM_RESOURCE_LOG(error) << buffer.str(); throw std::runtime_error(buffer.str()); } @@ -1347,7 +1347,7 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte } if (response.images.empty()) { - VIAM_SDK_LOG(error) << "[get_images] error: no camera sources matched the filter"; + VIAM_RESOURCE_LOG(error) << "[get_images] error: no camera sources matched the filter"; return response; } @@ -1357,7 +1357,7 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte if (colorTS > 0 && depthTS > 0) { if (colorTS != depthTS) { - VIAM_SDK_LOG(info) << "color and depth timestamps differ, defaulting to " + VIAM_RESOURCE_LOG(info) << "color and depth timestamps differ, defaulting to " "older of the two" << "color timestamp was " << colorTS << " depth timestamp was " << depthTS; } @@ -1371,10 +1371,10 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte std::chrono::microseconds latestTimestamp(timestamp); response.metadata.captured_at = vsdk::time_pt{std::chrono::duration_cast(latestTimestamp)}; - VIAM_SDK_LOG(debug) << "[get_images] end"; + VIAM_RESOURCE_LOG(debug) << "[get_images] end"; return response; } catch (const std::exception& e) { - VIAM_SDK_LOG(error) << "[get_images] error: " << e.what(); + VIAM_RESOURCE_LOG(error) << "[get_images] error: " << e.what(); throw std::runtime_error("failed to create images: " + std::string(e.what())); } } @@ -1411,14 +1411,14 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { dev->started = false; } - VIAM_SDK_LOG(info) << "Updating device firmware..."; + VIAM_RESOURCE_LOG(info) << "Updating device firmware..."; try { updateFirmware(dev, ob_ctx_); firmware_version_ = minFirmwareVer; } catch (const std::exception& e) { std::ostringstream buffer; buffer << "firmware update failed: " << e.what(); - VIAM_SDK_LOG(error) << buffer.str(); + VIAM_RESOURCE_LOG(error) << buffer.str(); resp.emplace(firmware_key, buffer.str()); dev->pipe->start(dev->config, frameCallback(serial_number)); dev->started = true; @@ -1433,7 +1433,7 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { return resp; } else if (key == "dump_pcl_files") { if (!value.is_a()) { - VIAM_SDK_LOG(error) << "[do_command] dump_pcl_files: expected bool, got " << value.kind(); + VIAM_RESOURCE_LOG(error) << "[do_command] dump_pcl_files: expected bool, got " << value.kind(); return vsdk::ProtoStruct{{"error", "expected bool"}}; } dev->dumpPCLFiles = value.get_unchecked(); @@ -1476,9 +1476,9 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { } } // unlock devices_by_serial_mu_ if (call_get_properties) { - VIAM_SDK_LOG(info) << "[do_command] calling get_properties"; + VIAM_RESOURCE_LOG(info) << "[do_command] calling get_properties"; auto const props = get_properties(); - VIAM_SDK_LOG(info) << "[do_command] get_properties called"; + VIAM_RESOURCE_LOG(info) << "[do_command] get_properties called"; vsdk::ProtoStruct resp; resp["supports_pcd"] = props.supports_pcd; resp["intrinsic_parameters"] = vsdk::ProtoStruct{{"width_px", props.intrinsic_parameters.width_px}, @@ -1495,18 +1495,18 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { } distortion_params["parameters"] = viam::sdk::ProtoValue(proto_params); resp["distortion_parameters"] = distortion_params; - VIAM_SDK_LOG(info) << "[do_command] get_properties returning"; + VIAM_RESOURCE_LOG(info) << "[do_command] get_properties returning"; return resp; } } catch (const std::exception& e) { - VIAM_SDK_LOG(error) << service_name << ": exception caught: " << e.what(); + VIAM_RESOURCE_LOG(error) << service_name << ": exception caught: " << e.what(); } return vsdk::ProtoStruct{}; } vsdk::Camera::point_cloud Orbbec::get_point_cloud(std::string mime_type, const vsdk::ProtoStruct& extra) { try { - VIAM_SDK_LOG(debug) << "[get_point_cloud] start"; + VIAM_RESOURCE_LOG(debug) << "[get_point_cloud] start"; std::string serial_number; { const std::lock_guard lock(serial_number_mu_); @@ -1600,12 +1600,12 @@ vsdk::Camera::point_cloud Orbbec::get_point_cloud(std::string mime_type, const v if (std::filesystem::exists(dir_path) && std::filesystem::is_directory(dir_path)) { outfile_name << viam_module_data << "/pointcloud_" << timestamp << ".pcd"; } else { - VIAM_SDK_LOG(warn) << "VIAM_MODULE_DATA is set to " << viam_module_data + VIAM_RESOURCE_LOG(warn) << "VIAM_MODULE_DATA is set to " << viam_module_data << " but is not a valid directory, using current working directory to store PCD file"; outfile_name << "pointcloud_" << timestamp << ".pcd"; } } else { - VIAM_SDK_LOG(warn) << "VIAM_MODULE_DATA is not set, using current working directory to store PCD file"; + VIAM_RESOURCE_LOG(warn) << "VIAM_MODULE_DATA is not set, using current working directory to store PCD file"; outfile_name << "pointcloud_" << timestamp << ".pcd"; } std::ofstream outfile(outfile_name.str(), std::ios::out | std::ios::binary); @@ -1615,13 +1615,13 @@ vsdk::Camera::point_cloud Orbbec::get_point_cloud(std::string mime_type, const v std::filesystem::path absolute_path = std::filesystem::absolute(file_path); std::string absolute_path_str = absolute_path.string(); outfile.close(); - VIAM_SDK_LOG(info) << "[get_point_cloud] wrote PCD to location: " << absolute_path_str; + VIAM_RESOURCE_LOG(info) << "[get_point_cloud] wrote PCD to location: " << absolute_path_str; } - VIAM_SDK_LOG(debug) << "[get_point_cloud] end"; + VIAM_RESOURCE_LOG(debug) << "[get_point_cloud] end"; return vsdk::Camera::point_cloud{kPcdMimeType, data}; } catch (const std::exception& e) { - VIAM_SDK_LOG(error) << "[get_point_cloud] error: " << e.what(); + VIAM_RESOURCE_LOG(error) << "[get_point_cloud] error: " << e.what(); throw std::runtime_error("failed to create pointcloud: " + std::string(e.what())); } } @@ -1844,7 +1844,7 @@ void deviceChangedCallback(const std::shared_ptr removedList, co } } } catch (ob::Error& e) { - std::cerr << "setDeviceChangedCallback\n" + VIAM_SDK_LOG(error) << "setDeviceChangedCallback\n" << "function:" << e.getFunction() << "\nargs:" << e.getArgs() << "\nname:" << e.getName() << "\nmessage:" << e.what() << "\ntype:" << e.getExceptionType() << std::endl; } @@ -1853,16 +1853,70 @@ void deviceChangedCallback(const std::shared_ptr removedList, co void startOrbbecSDK(ob::Context& ctx) { ctx.setDeviceChangedCallback(deviceChangedCallback); - std::shared_ptr devList = ctx.queryDeviceList(); - int devCount = devList->getCount(); - for (size_t i = 0; i < devCount; i++) { - if (i == 0) { - VIAM_SDK_LOG(info) << "devCount: " << devCount << "\n"; + // Enable network device enumeration for Ethernet cameras (like Gemini 335Le) + try { + ctx.enableNetDeviceEnumeration(true); + VIAM_SDK_LOG(info) << "Enabled network device enumeration for Ethernet cameras"; + } catch (ob::Error& e) { + VIAM_SDK_LOG(warn) << "Failed to enable network device enumeration: " << e.what(); + } catch (const std::exception& e) { + VIAM_SDK_LOG(warn) << "Failed to enable network device enumeration: " << e.what(); + } + + try { + // Use SDK's native device discovery (works for both USB and network devices) + std::shared_ptr devList = ctx.queryDeviceList(); + int devCount = devList->getCount(); + + if (devCount == 0) { + VIAM_SDK_LOG(warn) << "No Orbbec devices found"; + return; } - std::shared_ptr dev = devList->getDevice(i); - std::shared_ptr info = dev->getDeviceInfo(); - printDeviceInfo(info); - registerDevice(info->getSerialNumber(), dev); + + VIAM_SDK_LOG(info) << "Found " << devCount << " Orbbec devices"; + + for (size_t i = 0; i < devCount; i++) { + try { + // Get device information from DeviceList without triggering USB control transfers + std::string deviceName = devList->name(i); + std::string serialNumber = devList->serialNumber(i); + std::string connectionType = devList->connectionType(i); + std::string ipAddress = devList->ipAddress(i); + + std::stringstream deviceInfoString; + deviceInfoString << "Device " << (i + 1) << " - Name: " << deviceName + << ", Serial: " << serialNumber + << ", Connection: " << connectionType; + if(!ipAddress.empty()) { + deviceInfoString << ", IP: " << ipAddress; + } + VIAM_SDK_LOG(info) << deviceInfoString.str(); + + // Only create device if we can get the serial number + if (!serialNumber.empty()) { + std::shared_ptr dev = devList->getDevice(i); + registerDevice(serialNumber, dev); + } else { + VIAM_SDK_LOG(warn) << "Skipping device " << (i + 1) << " - no serial number available"; + } + } catch (ob::Error& deviceError) { + VIAM_SDK_LOG(warn) << "Failed to process device " << (i + 1) << ": " << deviceError.what(); + // Continue with other devices even if one fails + } + } + } catch (ob::Error& e) { + VIAM_SDK_LOG(error) << "Failed to query Orbbec devices: " << e.what() + << " (function: " << e.getFunction() + << ", args: " << e.getArgs() + << ", name: " << e.getName() + << ", type: " << e.getExceptionType() << ")"; + VIAM_SDK_LOG(warn) << "Continuing without Orbbec devices - check network connectivity for Ethernet cameras or USB connection for USB cameras"; + } catch (const std::exception& e) { + VIAM_SDK_LOG(error) << "Failed to query Orbbec devices: " << e.what(); + VIAM_SDK_LOG(warn) << "Continuing without Orbbec devices - check network connectivity for Ethernet cameras or USB connection for USB cameras"; + } catch (...) { + VIAM_SDK_LOG(error) << "Failed to query Orbbec devices: unknown error"; + VIAM_SDK_LOG(warn) << "Continuing without Orbbec devices - check network connectivity for Ethernet cameras or USB connection for USB cameras"; } } // ORBBEC SDK DEVICE REGISTRY END From 18dcdaa019a7c352af19ce7d2f3ef6379de643de Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 00:16:23 -0400 Subject: [PATCH 02/33] [[RSDK-12362] Add support for new Gemini in discovery and complete in the module --- CMakeLists.txt | 2 +- src/module/discovery.cpp | 14 +- src/module/main.cpp | 11 +- src/module/orbbec.cpp | 801 ++++++++++--------------- src/module/orbbec.hpp | 25 +- src/module/orbbec_firmware.cpp | 134 +++++ src/module/orbbec_firmware.hpp | 38 ++ src/module/orbbec_windows_registry.cpp | 135 +++++ src/module/orbbec_windows_registry.hpp | 33 + 9 files changed, 692 insertions(+), 501 deletions(-) create mode 100644 src/module/orbbec_firmware.cpp create mode 100644 src/module/orbbec_firmware.hpp create mode 100644 src/module/orbbec_windows_registry.cpp create mode 100644 src/module/orbbec_windows_registry.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8d7ab749..7075c69e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,7 +23,7 @@ else() endif() add_executable(orbbec-cli src/cli/main.cpp) -add_executable(orbbec-module src/module/main.cpp src/module/orbbec.cpp src/module/discovery.cpp src/module/encoding.cpp) +add_executable(orbbec-module src/module/main.cpp src/module/orbbec.cpp src/module/discovery.cpp src/module/encoding.cpp src/module/orbbec_firmware.cpp src/module/orbbec_windows_registry.cpp) target_compile_features(orbbec-cli PRIVATE cxx_std_17) target_compile_features(orbbec-module PRIVATE cxx_std_17) diff --git a/src/module/discovery.cpp b/src/module/discovery.cpp index 1032f9c7..7202a87c 100644 --- a/src/module/discovery.cpp +++ b/src/module/discovery.cpp @@ -57,8 +57,20 @@ std::vector OrbbecDiscovery::discover_resources(const vsdk vsdk::ProtoStruct attributes; attributes.emplace("serial_number", serialNumber); + // Detect model and create appropriate resource + std::string viamModelSuffix; + try { + const auto& modelConfig = orbbec::OrbbecModelConfig::forDevice(deviceName); + viamModelSuffix = modelConfig.viam_model_suffix; + } catch (const std::runtime_error& e) { + VIAM_RESOURCE_LOG(warn) << "Skipping unsupported camera: " << deviceName; + continue; + } + vsdk::ResourceConfig config( - "camera", std::move(name.str()), "viam", attributes, "rdk:component:camera", orbbec::Orbbec::model, vsdk::log_level::info); + "camera", std::move(name.str()), "viam", attributes, "rdk:component:camera", + vsdk::Model("viam", "orbbec", viamModelSuffix), + vsdk::log_level::info); configs.push_back(config); VIAM_RESOURCE_LOG(info) << "Successfully configured device " << (i + 1) << " with serial: " << serialNumber; diff --git a/src/module/main.cpp b/src/module/main.cpp index e386a832..91900193 100644 --- a/src/module/main.cpp +++ b/src/module/main.cpp @@ -17,11 +17,18 @@ std::vector> create_all_model_registrat registrations.push_back(std::make_shared( vsdk::API::get(), - orbbec::Orbbec::model, + orbbec::Orbbec::model_astra2, [ctx](vsdk::Dependencies deps, vsdk::ResourceConfig config) { return std::make_unique(std::move(deps), std::move(config), ctx); }, - orbbec::Orbbec::validate)); + orbbec::Orbbec::validateAstra2)); + registrations.push_back(std::make_shared( + vsdk::API::get(), + orbbec::Orbbec::model_gemini_335le, + [ctx](vsdk::Dependencies deps, vsdk::ResourceConfig config) { + return std::make_unique(std::move(deps), std::move(config), ctx); + }, + orbbec::Orbbec::validateGemini335Le)); registrations.push_back(std::make_shared( vsdk::API::get(), discovery::OrbbecDiscovery::model, [ctx](vsdk::Dependencies deps, vsdk::ResourceConfig config) { diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 568c76d6..a0f07bc9 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -15,13 +15,8 @@ #include "orbbec.hpp" #include "device_control.hpp" #include "encoding.hpp" -#ifdef _WIN32 -#include -#endif - -#include -#include -#include +#include "orbbec_firmware.hpp" +#include "orbbec_windows_registry.hpp" #include #include @@ -46,7 +41,6 @@ #include #include -#include #include #include @@ -54,20 +48,12 @@ namespace orbbec { namespace vsdk = ::viam::sdk; -vsdk::Model Orbbec::model("viam", "orbbec", "astra2"); +vsdk::Model Orbbec::model_astra2("viam", "orbbec", "astra2"); +vsdk::Model Orbbec::model_gemini_335le("viam", "orbbec", "gemini_335le"); std::unordered_set const Orbbec::supported_color_formats{"RGB", "MJPG"}; std::unordered_set const Orbbec::supported_depth_formats{"Y16"}; std::string const Orbbec::default_color_format = "MJPG"; std::string const Orbbec::default_depth_format = "Y16"; -Resolution const Orbbec::default_color_resolution{1280, 720}; -Resolution const Orbbec::default_depth_resolution{1600, 1200}; -std::map>, std::greater> const - Orbbec::color_to_depth_supported_resolutions{{{1920, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, - {{1440, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, - {{1280, 720}, {{1600, 1200}, {800, 600}, {400, 300}}}, - {{800, 600}, {{800, 600}, {400, 300}}}, - {{640, 480}, {{800, 600}, {400, 300}}}, - {{640, 360}, {{800, 600}, {400, 300}}}}; // CONSTANTS BEGIN const std::string kColorSourceName = "color"; @@ -77,12 +63,46 @@ const std::string kDepthSourceName = "depth"; const std::string kDepthMimeTypeViamDep = "image/vnd.viam.dep"; const std::string kPcdMimeType = "pointcloud/pcd"; // If the firmwareUrl is changed to a new version, also change the minFirmwareVer const. -const std::string firmwareUrl = "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Astra2_Release_2.8.20.zip"; -const std::string minFirmwareVer = "2.8.20"; constexpr char service_name[] = "viam_orbbec"; const float mmToMeterMultiple = 0.001; const uint64_t maxFrameAgeUs = 1e6; // time until a frame is considered stale, in microseconds (equal to 1 sec) + +// Model configurations +namespace { + const OrbbecModelConfig ASTRA2_CONFIG{ + .model_name = "Astra 2", + .viam_model_suffix = "astra2", + .default_color_resolution = {1280, 720}, + .default_depth_resolution = {1600, 1200}, + .firmware_url = "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Astra2_Release_2.8.20.zip", + .min_firmware_version = "2.8.20", + .color_to_depth_supported_resolutions = {{{1920, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, + {{1440, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, + {{1280, 720}, {{1600, 1200}, {800, 600}, {400, 300}}}, + {{800, 600}, {{800, 600}, {400, 300}}}, + {{640, 480}, {{800, 600}, {400, 300}}}, + {{640, 360}, {{800, 600}, {400, 300}}}} + }; + + const OrbbecModelConfig GEMINI_335LE_CONFIG{ + .model_name = "Gemini 335Le", + .viam_model_suffix = "gemini_335le", + .default_color_resolution = {1280, 800}, + .default_depth_resolution = {848, 530}, + .firmware_url = "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Gemini330_Release_1.6.00.zip", + .min_firmware_version = "1.5.31", + .color_to_depth_supported_resolutions = {{{1280, 800}, {{848, 530}, {424, 265}}}, + {{640, 400}, {{848, 530}, {424, 265}}}} + }; +} + +const OrbbecModelConfig& OrbbecModelConfig::forDevice(const std::string& device_name) { + if (device_name.find("Astra2") != std::string::npos || device_name.find("Astra 2") != std::string::npos) return ASTRA2_CONFIG; + if (device_name.find("Gemini 335Le") != std::string::npos) return GEMINI_335LE_CONFIG; + throw std::runtime_error("Unsupported Orbbec camera model: " + device_name); +} + // CONSTANTS END // STRUCTS BEGIN @@ -141,26 +161,20 @@ std::unordered_map& config_by_serial() { // GLOBALS END // HELPERS BEGIN -void checkFirmwareVersion(const std::string version) { +void checkFirmwareVersion(const std::string version, const std::string& minFirmwareVer, const std::string& modelName) { int major = 0, minor = 0, patch = 0; int requiredMajor = 0, requiredMinor = 0, requiredPatch = 0; + // ignore any trailing text in the case of a beta or RC version sscanf(version.c_str(), "%d.%d.%d*s", &major, &minor, &patch); sscanf(minFirmwareVer.c_str(), "%d.%d.%d", &requiredMajor, &requiredMinor, &requiredPatch); if ((major < requiredMajor) || (major == requiredMajor && minor < requiredMinor) || (major == requiredMajor && minor == requiredMinor && patch < requiredPatch)) { - throw std::runtime_error("Unsupported firmware version. Required: >= 2.8.20, Current: " + version + + throw std::runtime_error("Unsupported firmware version for " + modelName + ". Required: >= " + minFirmwareVer + ", Current: " + version + ". Call update_firmware command to upgrade."); } } -// Convert integer to uppercase 4-digit hex string -std::string uint16ToHex(uint16_t value) { - std::stringstream ss; - ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0') << value; - return ss.str(); -} - uint64_t getNowUs() { return std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); } @@ -169,9 +183,123 @@ uint64_t timeSinceFrameUs(uint64_t nowUs, uint64_t imageTimeUs) { if (nowUs > imageTimeUs) { return nowUs - imageTimeUs; } - return 0; + return imageTimeUs - nowUs; +} + +namespace { + +// Helper function to format error messages +template +std::string formatError(Args&&... args) { + std::ostringstream buffer; + (buffer << ... << args); + return buffer.str(); +} + +// Validate color frame format and timestamp +void validateColorFrame(std::shared_ptr color, + const std::optional& device_format_opt) { + if (color == nullptr) { + throw std::runtime_error("no color frame"); + } + + // Format validation + std::string frameFormat = ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()); + if (Orbbec::supported_color_formats.count(frameFormat) == 0) { + std::ostringstream buffer; + buffer << "unsupported color format: " << frameFormat << ", supported: "; + for (const auto& fmt : Orbbec::supported_color_formats) { + buffer << fmt << " "; + } + throw std::runtime_error(buffer.str()); + } + + // Timestamp validation + uint64_t nowUs = getNowUs(); + uint64_t diff = timeSinceFrameUs(nowUs, color->getSystemTimeStampUs()); + if (diff > maxFrameAgeUs) { + throw std::runtime_error(formatError("no recent color frame: check connection, diff: ", diff, "us")); + } + + // Config format validation + if (device_format_opt.has_value() && device_format_opt->color_format.has_value()) { + if (frameFormat != device_format_opt->color_format.value()) { + throw std::runtime_error(formatError("color format mismatch, expected ", + device_format_opt->color_format.value(), + " got ", frameFormat)); + } + } else if (frameFormat != Orbbec::default_color_format) { + throw std::runtime_error(formatError("color format mismatch, expected ", + Orbbec::default_color_format, " got ", frameFormat)); + } } +// Validate depth frame format and timestamp +void validateDepthFrame(std::shared_ptr depth, + const std::optional& device_format_opt) { + if (depth == nullptr) { + throw std::runtime_error("no depth frame"); + } + + // Format validation + std::string frameFormat = ob::TypeHelper::convertOBFormatTypeToString(depth->getFormat()); + if (Orbbec::supported_depth_formats.count(frameFormat) == 0) { + std::ostringstream buffer; + buffer << "unsupported depth format: " << frameFormat << ", supported: "; + for (const auto& fmt : Orbbec::supported_depth_formats) { + buffer << fmt << " "; + } + throw std::runtime_error(buffer.str()); + } + + // Timestamp validation + uint64_t nowUs = getNowUs(); + uint64_t diff = timeSinceFrameUs(nowUs, depth->getSystemTimeStampUs()); + if (diff > maxFrameAgeUs) { + throw std::runtime_error(formatError("no recent depth frame: check connection, diff: ", diff, "us")); + } + + // Config format validation + if (device_format_opt.has_value() && device_format_opt->depth_format.has_value()) { + if (frameFormat != device_format_opt->depth_format.value()) { + throw std::runtime_error(formatError("depth format mismatch, expected ", + device_format_opt->depth_format.value(), + " got ", frameFormat)); + } + } else if (frameFormat != Orbbec::default_depth_format) { + throw std::runtime_error(formatError("depth format mismatch, expected ", + Orbbec::default_depth_format, " got ", frameFormat)); + } +} + +// Encode color frame to raw_image +vsdk::Camera::raw_image encodeColorFrame(std::shared_ptr color) { + vsdk::Camera::raw_image image; + image.source_name = kColorSourceName; + + std::uint8_t* colorData = (std::uint8_t*)color->getData(); + if (colorData == nullptr) { + throw std::runtime_error("color data is null"); + } + uint32_t colorDataSize = color->dataSize(); + + if (color->getFormat() == OB_FORMAT_MJPG) { + image.mime_type = kColorMimeTypeJPEG; + image.bytes.assign(colorData, colorData + colorDataSize); + } else if (color->getFormat() == OB_FORMAT_RGB) { + image.mime_type = kColorMimeTypePNG; + auto width = color->getStreamProfile()->as()->getWidth(); + auto height = color->getStreamProfile()->as()->getHeight(); + image.bytes = encoding::encode_to_png(colorData, width, height); + } else { + throw std::runtime_error(formatError("unsupported color format: ", + ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()))); + } + return image; +} + +} // anonymous namespace + std::vector RGBPointsToPCD(std::shared_ptr frame, float scale) { int numPoints = frame->dataSize() / sizeof(OBColorPoint); @@ -295,7 +423,8 @@ bool checkIfSupportHWD2CAlign(std::shared_ptr pipe, // create a config for hardware depth-to-color alignment std::shared_ptr createHwD2CAlignConfig(std::shared_ptr pipe, std::optional deviceRes, - std::optional deviceFormat) { + std::optional deviceFormat, + const OrbbecModelConfig* modelConfig) { auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); if (deviceRes.has_value()) { @@ -326,8 +455,8 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr colorVsp->getHeight() != deviceRes->color_resolution->height) { continue; } - } else if (colorVsp->getWidth() != Orbbec::default_color_resolution.width || - colorVsp->getHeight() != Orbbec::default_color_resolution.height) { + } else if (colorVsp->getWidth() != modelConfig->default_color_resolution.width || + colorVsp->getHeight() != modelConfig->default_color_resolution.height) { continue; } @@ -354,8 +483,8 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr depthVsp->getHeight() != deviceRes->depth_resolution->height) { continue; } - } else if (depthVsp->getWidth() != Orbbec::default_depth_resolution.width || - depthVsp->getHeight() != Orbbec::default_depth_resolution.height) { + } else if (depthVsp->getWidth() != modelConfig->default_depth_resolution.width || + depthVsp->getHeight() != modelConfig->default_depth_resolution.height) { continue; } @@ -395,156 +524,6 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr return nullptr; } -size_t writeFileCallback(void* contents, size_t size, size_t nmemb, void* userp) { - auto* buffer = static_cast*>(userp); - size_t totalSize = size * nmemb; - buffer->insert(buffer->end(), static_cast(contents), static_cast(contents) + totalSize); - return totalSize; -} - -template -struct Cleanup { - using pointer_type = std::tuple_element_t<0, boost::callable_traits::args_t>; - using value_type = std::remove_pointer_t; - - void operator()(pointer_type p) { - if (p != nullptr) { - cleanup_fp(p); - } - } -}; - -template -using CleanupPtr = std::unique_ptr::value_type, Cleanup>; - -void updateFirmware(std::unique_ptr& my_dev, std::shared_ptr ctx) { -// On linux, orbbec reccomends to set libuvc backend for firmware update -#if defined(__linux__) - ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC); -#endif - - CleanupPtr curl(curl_easy_init()); - if (!curl) { - throw std::invalid_argument("curl easy init failed"); - } - - // Download the firmware and write it to a buffer - std::vector zipBuffer; - curl_easy_setopt(curl.get(), CURLOPT_URL, firmwareUrl.c_str()); - curl_easy_setopt(curl.get(), CURLOPT_FOLLOWLOCATION, 1L); - curl_easy_setopt(curl.get(), CURLOPT_WRITEFUNCTION, writeFileCallback); - curl_easy_setopt(curl.get(), CURLOPT_WRITEDATA, &zipBuffer); - CURLcode res = curl_easy_perform(curl.get()); - if (res != CURLE_OK) { - std::ostringstream buffer; - buffer << "curl early perform failed: " << curl_easy_strerror(res); - throw std::invalid_argument(buffer.str()); - } - - std::vector binData; - zip_error_t ziperror; - zip_error_init(&ziperror); - - zip_source_t* src = zip_source_buffer_create(zipBuffer.data(), zipBuffer.size(), 0, &ziperror); - if (!src) { - std::ostringstream buffer; - buffer << "failed to create zip buffer: " << zip_error_strerror(&ziperror); - throw std::runtime_error(buffer.str()); - } - - // Ensure src cleanup if zip_open fails - CleanupPtr srcCleanup(src); - - // If this succeeds, zip takes ownership of src, so src will be freed when zip_close is called. - zip_t* zip = zip_open_from_source(src, 0, &ziperror); - if (!zip) { - std::ostringstream buffer; - buffer << "failed to open zip from source: " << zip_error_strerror(&ziperror); - throw std::runtime_error(buffer.str()); - } - - srcCleanup.release(); - CleanupPtr zipCleanup(zip); - - if (zip_get_num_entries(zip, 0) != 1) { - throw std::runtime_error("unexpected number of files in firmware zip"); - } - - const char* fileName = zip_get_name(zip, 0, 0); - if (!fileName) { - throw std::runtime_error("couldn't get bin file name"); - } - - CleanupPtr binFile(zip_fopen(zip, fileName, 0)); - if (!binFile) { - throw std::runtime_error("failed to open the firmware bin file"); - } - - zip_stat_t stats; - zip_stat_init(&stats); - if (zip_stat(zip, fileName, 0, &stats) != 0) { - throw std::invalid_argument("failed to stat file"); - } - - binData.resize(stats.size); - zip_int64_t bytesRead = zip_fread(binFile.get(), binData.data(), stats.size); - if (bytesRead == -1) { - zip_error_t* err = zip_file_get_error(binFile.get()); - std::ostringstream buffer; - buffer << "failed to read bin: " << zip_error_strerror(err); - throw std::runtime_error(buffer.str()); - } - - if (bytesRead != stats.size) { - std::ostringstream buffer; - buffer << "failed to fully read binary file, file size: " << stats.size << "bytes read: " << bytesRead; - throw std::runtime_error(buffer.str()); - } - - auto firmwareUpdateCallback = [](OBFwUpdateState state, const char* message, uint8_t percent) { - switch (state) { - case STAT_VERIFY_SUCCESS: - std::cout << "Image file verification success\n"; - case STAT_FILE_TRANSFER: - std::cout << "File transfer in progress\n"; - break; - case STAT_DONE: - std::cout << "Update completed\n"; - break; - case STAT_IN_PROGRESS: - std::cout << "Upgrade in progress\n"; - break; - case STAT_START: - std::cout << "Starting the upgrade\n"; - break; - case STAT_VERIFY_IMAGE: - std::cout << "Verifying image file\n"; - break; - default: - std::cerr << "Unknown status or error\n"; - break; - } - std::cout << "Firmware update in progress: " << message << " upgrade " << static_cast(percent) << "% complete\n"; - }; - - bool executeAsync = false; - try { - my_dev->device->updateFirmwareFromData(binData.data(), binData.size(), std::move(firmwareUpdateCallback), executeAsync); - VIAM_SDK_LOG(info) << "firmware update successful!"; - } catch (...) { - // Reset UVC backend type before re-throwing -#if defined(__linux__) - ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_AUTO); -#endif - throw; - } - - // Reset UVC backend type on success -#if defined(__linux__) - ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_AUTO); -#endif -} - auto frameCallback(const std::string& serialNumber) { return [serialNumber](std::shared_ptr frameSet) { if (frameSet->getCount() != 2) { @@ -599,7 +578,7 @@ auto frameCallback(const std::string& serialNumber) { }; } -void startDevice(std::string serialNumber) { +void startDevice(std::string serialNumber, const OrbbecModelConfig* modelConfig) { VIAM_SDK_LOG(info) << service_name << ": starting device " << serialNumber; std::lock_guard lock(devices_by_serial_mu()); auto search = devices_by_serial().find(serialNumber); @@ -609,19 +588,21 @@ void startDevice(std::string serialNumber) { throw std::invalid_argument(buffer.str()); } - // Check if the device is an Astra 2 + // Check if the device is a supported Orbbec camera (Astra 2 or Gemini 335Le) std::shared_ptr deviceInfo = search->second->device->getDeviceInfo(); - if (!strstr(deviceInfo->name(), "Astra 2")) { + std::string deviceName = deviceInfo->name(); + if (!strstr(deviceName.c_str(), "Astra 2") && !strstr(deviceName.c_str(), "Gemini 335Le")) { std::ostringstream buffer; - buffer << service_name << ": device " << serialNumber << " is not an Astra 2 (found: " << deviceInfo->name() << ")"; + buffer << service_name << ": device " << serialNumber << " is not a supported camera (found: " << deviceName << ")"; throw std::invalid_argument(buffer.str()); } + + VIAM_SDK_LOG(info) << "Device " << serialNumber << " is supported: " << deviceName; std::unique_ptr& my_dev = search->second; if (my_dev->started) { - std::ostringstream buffer; - buffer << service_name << ": unable to start already started device" << serialNumber; - throw std::invalid_argument(buffer.str()); + VIAM_SDK_LOG(info) << service_name << ": device " << serialNumber << " is already started, sharing existing instance"; + return; } VIAM_SDK_LOG(info) << "[startDevice] Configuring device resolution"; { @@ -638,7 +619,7 @@ void startDevice(std::string serialNumber) { << (format_opt.has_value() ? format_opt->to_string() : "not specifed"); if (resolution_opt.has_value() || format_opt.has_value()) { // Create the pipeline - auto config = createHwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt); + auto config = createHwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt, modelConfig); if (config == nullptr) { std::ostringstream buffer; buffer << service_name << ": device with serial number " << serialNumber @@ -768,7 +749,67 @@ void Orbbec::validate_sensor(std::pair const } } -std::vector Orbbec::validate(vsdk::ResourceConfig cfg) { +std::vector Orbbec::validateAstra2(vsdk::ResourceConfig cfg) { + auto attrs = cfg.attributes(); + + if (!attrs.count("serial_number")) { + throw std::invalid_argument("serial_number is a required argument"); + } + + if (!attrs["serial_number"].get()) { + throw std::invalid_argument("serial_number must be a string"); + } + + // We already established this is a string, so it's safe to call this + std::string const serial = attrs["serial_number"].get_unchecked(); + if (serial.empty()) { + throw std::invalid_argument("serial_number must be a non-empty string"); + } + + if (attrs.count("sensors")) { + auto sensors = attrs["sensors"].get(); + if (!sensors) { + throw std::invalid_argument("sensors must be a struct"); + } + if (sensors) { + for (auto const& sensor_pair : *sensors) { + validate_sensor(sensor_pair); + } + } + auto color_width_uint32 = + static_cast(*sensors->at("color").get()->at("width").get()); + auto color_height_uint32 = + static_cast(*sensors->at("color").get()->at("height").get()); + if (ASTRA2_CONFIG.color_to_depth_supported_resolutions.count({color_width_uint32, color_height_uint32}) == 0) { + std::ostringstream buffer; + buffer << "color resolution must be one of: "; + for (const auto& res : ASTRA2_CONFIG.color_to_depth_supported_resolutions) { + buffer << "{" << res.first.to_string() << "} "; + } + VIAM_SDK_LOG(error) << buffer.str(); + throw std::invalid_argument(buffer.str()); + } + auto depth_width_uint32 = + static_cast(*sensors->at("depth").get()->at("width").get()); + auto depth_height_uint32 = + static_cast(*sensors->at("depth").get()->at("height").get()); + if (ASTRA2_CONFIG.color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32}) + .count({depth_width_uint32, depth_height_uint32}) == 0) { + std::ostringstream buffer; + buffer << "color/depth resolution combination not supported, for color resolution " << "{" << color_width_uint32 << ", " + << color_height_uint32 << "}, depth resolution must be one of: "; + for (const auto& res : ASTRA2_CONFIG.color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32})) { + buffer << "{" << res.to_string() << "} "; + } + VIAM_SDK_LOG(error) << buffer.str(); + throw std::invalid_argument(buffer.str()); + } + } + + return {}; +} + +std::vector Orbbec::validateGemini335Le(vsdk::ResourceConfig cfg) { auto attrs = cfg.attributes(); if (!attrs.count("serial_number")) { @@ -799,10 +840,10 @@ std::vector Orbbec::validate(vsdk::ResourceConfig cfg) { static_cast(*sensors->at("color").get()->at("width").get()); auto color_height_uint32 = static_cast(*sensors->at("color").get()->at("height").get()); - if (color_to_depth_supported_resolutions.count({color_width_uint32, color_height_uint32}) == 0) { + if (GEMINI_335LE_CONFIG.color_to_depth_supported_resolutions.count({color_width_uint32, color_height_uint32}) == 0) { std::ostringstream buffer; buffer << "color resolution must be one of: "; - for (const auto& res : color_to_depth_supported_resolutions) { + for (const auto& res : GEMINI_335LE_CONFIG.color_to_depth_supported_resolutions) { buffer << "{" << res.first.to_string() << "} "; } VIAM_SDK_LOG(error) << buffer.str(); @@ -812,12 +853,12 @@ std::vector Orbbec::validate(vsdk::ResourceConfig cfg) { static_cast(*sensors->at("depth").get()->at("width").get()); auto depth_height_uint32 = static_cast(*sensors->at("depth").get()->at("height").get()); - if (color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32}) + if (GEMINI_335LE_CONFIG.color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32}) .count({depth_width_uint32, depth_height_uint32}) == 0) { std::ostringstream buffer; buffer << "color/depth resolution combination not supported, for color resolution " << "{" << color_width_uint32 << ", " << color_height_uint32 << "}, depth resolution must be one of: "; - for (const auto& res : color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32})) { + for (const auto& res : GEMINI_335LE_CONFIG.color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32})) { buffer << "{" << res.to_string() << "} "; } VIAM_SDK_LOG(error) << buffer.str(); @@ -887,18 +928,24 @@ Orbbec::Orbbec(vsdk::Dependencies deps, vsdk::ResourceConfig cfg, std::shared_pt applyExperimentalConfig(my_dev, cfg.attributes()); } - startDevice(serial_number_); + startDevice(serial_number_, model_config_); { std::lock_guard lock(serial_by_resource_mu()); serial_by_resource()[config->resource_name] = serial_number_; } // set firmware version member variable + { std::lock_guard lock(devices_by_serial_mu()); auto search = devices_by_serial().find(serial_number_); if (search != devices_by_serial().end()) { firmware_version_ = search->second->device->getDeviceInfo()->firmwareVersion(); + + // Detect model and set model_config_ + std::shared_ptr deviceInfo = search->second->device->getDeviceInfo(); + model_config_ = &OrbbecModelConfig::forDevice(deviceInfo->name()); + VIAM_SDK_LOG(info) << "Detected model: " << model_config_->model_name; } } @@ -964,7 +1011,7 @@ void Orbbec::reconfigure(const vsdk::Dependencies& deps, const vsdk::ResourceCon frame_set_by_serial().erase(prev_serial_number); } - startDevice(new_serial_number); + startDevice(new_serial_number, model_config_); { std::lock_guard lock(serial_by_resource_mu()); serial_by_resource()[new_resource_name] = new_serial_number; @@ -993,7 +1040,7 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro serial_number = serial_number_; } - checkFirmwareVersion(firmware_version_); + // checkFirmwareVersion(firmware_version_); std::shared_ptr fs = nullptr; { @@ -1005,21 +1052,7 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro fs = search->second; } std::shared_ptr color = fs->getFrame(OB_FRAME_COLOR); - if (color == nullptr) { - throw std::invalid_argument("no color frame"); - } - - if (supported_color_formats.count(ob::TypeHelper::convertOBFormatTypeToString(color->getFormat())) == 0) { - std::ostringstream buffer; - buffer << "unsupported color format: " << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()) - << ", supported formats: "; - for (const auto& fmt : supported_color_formats) { - buffer << fmt << " "; - } - VIAM_RESOURCE_LOG(error) << buffer.str(); - throw std::invalid_argument(buffer.str()); - } - + std::optional res_format_opt; { std::lock_guard lock(config_by_serial_mu()); @@ -1029,56 +1062,8 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro res_format_opt = config_by_serial().at(serial_number).device_format; } - if (res_format_opt.has_value()) { - if (res_format_opt->color_format.has_value() && - ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()) != res_format_opt->color_format.value()) { - std::ostringstream buffer; - buffer << "color frame format " << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()) - << " does not match configured color format " << res_format_opt->color_format.value(); - VIAM_RESOURCE_LOG(error) << buffer.str(); - throw std::invalid_argument(buffer.str()); - } - } else if (ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()) != Orbbec::default_color_format) { - std::ostringstream buffer; - buffer << "color frame format " << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()) - << " does not match default color format " << Orbbec::default_color_format; - VIAM_RESOURCE_LOG(error) << buffer.str(); - throw std::invalid_argument(buffer.str()); - } - - // If the image's timestamp is older than a second throw error, this - // indicates we no longer have a working camera. - uint64_t nowUs = getNowUs(); - uint64_t diff = timeSinceFrameUs(nowUs, color->getSystemTimeStampUs()); - if (diff > maxFrameAgeUs) { - std::ostringstream buffer; - buffer << "no recent color frame: check USB connection, diff: " << diff << "us"; - throw std::invalid_argument(buffer.str()); - } - - vsdk::Camera::raw_image response; - response.source_name = kColorSourceName; - - std::uint8_t* colorData = (std::uint8_t*)color->getData(); - if (colorData == nullptr) { - throw std::runtime_error("[get_image] color data is null"); - } - uint32_t colorDataSize = color->dataSize(); - - if (color->getFormat() == OB_FORMAT_MJPG) { - response.mime_type = kColorMimeTypeJPEG; - response.bytes.assign(colorData, colorData + colorDataSize); - } else if (color->getFormat() == OB_FORMAT_RGB) { - response.mime_type = kColorMimeTypePNG; - auto width = color->getStreamProfile()->as()->getWidth(); - auto height = color->getStreamProfile()->as()->getHeight(); - response.bytes = encoding::encode_to_png(colorData, width, height); - } else { - std::ostringstream buffer; - buffer << "[get_image] unsupported color format: " << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()); - VIAM_RESOURCE_LOG(error) << buffer.str(); - throw std::invalid_argument(buffer.str()); - } + validateColorFrame(color, res_format_opt); + vsdk::Camera::raw_image response = encodeColorFrame(color); VIAM_RESOURCE_LOG(debug) << "[get_image] end"; return response; } catch (const std::exception& e) { @@ -1182,7 +1167,7 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte serial_number = serial_number_; } - checkFirmwareVersion(firmware_version_); + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); std::shared_ptr fs = nullptr; { @@ -1229,109 +1214,15 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte if (should_process_color) { color = fs->getFrame(OB_FRAME_COLOR); - if (color == nullptr) { - throw std::runtime_error("no color frame"); - } - if (supported_color_formats.count(ob::TypeHelper::convertOBFormatTypeToString(color->getFormat())) == 0) { - std::ostringstream buffer; - buffer << "[get_images] unsupported color format: " << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()) - << ", supported: "; - for (const auto& format : supported_color_formats) { - buffer << format << " "; - } - - VIAM_RESOURCE_LOG(error) << buffer.str(); - throw std::runtime_error(buffer.str()); - } - - uint64_t diff = timeSinceFrameUs(nowUs, color->getSystemTimeStampUs()); - if (diff > maxFrameAgeUs) { - std::ostringstream buffer; - buffer << "no recent color frame: check USB connection, diff: " << diff << "us"; - throw std::runtime_error(buffer.str()); - } - - if (device_format_opt.has_value()) { - if (device_format_opt->color_format.has_value()) { - if (ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()) != device_format_opt->color_format.value()) { - std::ostringstream buffer; - buffer << "color format mismatch, expected " << device_format_opt->color_format.value() << " got " - << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()); - throw std::runtime_error(buffer.str()); - } - } - } else { - if (ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()) != Orbbec::default_color_format) { - std::ostringstream buffer; - buffer << "color format mismatch, expected " << Orbbec::default_color_format << " got " - << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()); - throw std::runtime_error(buffer.str()); - } - } - - std::uint8_t* colorData = (std::uint8_t*)color->getData(); - if (colorData == nullptr) { - throw std::runtime_error("[get_image] color data is null"); - } - std::uint32_t colorDataSize = color->dataSize(); - - vsdk::Camera::raw_image color_image; - - if (color->getFormat() == OB_FORMAT_MJPG) { - color_image.mime_type = kColorMimeTypeJPEG; - color_image.bytes.assign(colorData, colorData + colorDataSize); - response.images.emplace_back(std::move(color_image)); - } else if (color->getFormat() == OB_FORMAT_RGB) { - color_image.mime_type = kColorMimeTypePNG; - auto width = color->getStreamProfile()->as()->getWidth(); - auto height = color->getStreamProfile()->as()->getHeight(); - color_image.bytes = encoding::encode_to_png(colorData, width, height); - response.images.emplace_back(std::move(color_image)); - } else { - std::ostringstream buffer; - buffer << "[get_images] unsupported color format: " << ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()); - VIAM_RESOURCE_LOG(error) << buffer.str(); - throw std::runtime_error(buffer.str()); - } + validateColorFrame(color, device_format_opt); + + vsdk::Camera::raw_image color_image = encodeColorFrame(color); + response.images.emplace_back(std::move(color_image)); } if (should_process_depth) { depth = fs->getFrame(OB_FRAME_DEPTH); - if (depth == nullptr) { - throw std::runtime_error("no depth frame"); - } - if (supported_depth_formats.count(ob::TypeHelper::convertOBFormatTypeToString(depth->getFormat())) == 0) { - std::ostringstream buffer; - buffer << "[get_images] unsupported depth format: " << ob::TypeHelper::convertOBFormatTypeToString(depth->getFormat()) - << ", supported: "; - for (const auto& format : supported_depth_formats) { - buffer << format << " "; - } - VIAM_RESOURCE_LOG(error) << buffer.str(); - throw std::runtime_error(buffer.str()); - } - - uint64_t diff = timeSinceFrameUs(nowUs, depth->getSystemTimeStampUs()); - if (diff > maxFrameAgeUs) { - std::ostringstream buffer; - buffer << "no recent depth frame: check USB connection, diff: " << diff << "us"; - throw std::runtime_error(buffer.str()); - } - if (device_format_opt->depth_format.has_value()) { - if (ob::TypeHelper::convertOBFormatTypeToString(depth->getFormat()) != device_format_opt->depth_format.value()) { - std::ostringstream buffer; - buffer << "depth format mismatch, expected " << device_format_opt->depth_format.value() << " got " - << ob::TypeHelper::convertOBFormatTypeToString(depth->getFormat()); - throw std::runtime_error(buffer.str()); - } - } else { - if (ob::TypeHelper::convertOBFormatTypeToString(depth->getFormat()) != Orbbec::default_depth_format) { - std::ostringstream buffer; - buffer << "depth format mismatch, expected " << Orbbec::default_depth_format << " got " - << ob::TypeHelper::convertOBFormatTypeToString(depth->getFormat()); - throw std::runtime_error(buffer.str()); - } - } + validateDepthFrame(depth, device_format_opt); unsigned char* depthData = (unsigned char*)depth->getData(); if (depthData == nullptr) { @@ -1400,9 +1291,9 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { for (auto const& [key, value] : command) { if (key == firmware_key) { vsdk::ProtoStruct resp = viam::sdk::ProtoStruct{}; - if (firmware_version_.find(minFirmwareVer) != std::string::npos) { + if (firmware_version_.find(model_config_->min_firmware_version) != std::string::npos) { std::ostringstream buffer; - buffer << "device firmware already on version " << minFirmwareVer; + buffer << "device firmware already on version " << model_config_->min_firmware_version; resp.emplace(firmware_key, buffer.str()); break; } @@ -1413,8 +1304,12 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { VIAM_RESOURCE_LOG(info) << "Updating device firmware..."; try { - updateFirmware(dev, ob_ctx_); - firmware_version_ = minFirmwareVer; + if (model_config_->firmware_url.has_value()) { + firmware::updateFirmware(dev, ob_ctx_, model_config_->firmware_url.value()); + } else { + throw std::runtime_error("Firmware update not supported for this model"); + } + firmware_version_ = model_config_->min_firmware_version; } catch (const std::exception& e) { std::ostringstream buffer; buffer << "firmware update failed: " << e.what(); @@ -1513,7 +1408,7 @@ vsdk::Camera::point_cloud Orbbec::get_point_cloud(std::string mime_type, const v serial_number = serial_number_; } - checkFirmwareVersion(firmware_version_); + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); std::shared_ptr fs = nullptr; { @@ -1526,29 +1421,10 @@ vsdk::Camera::point_cloud Orbbec::get_point_cloud(std::string mime_type, const v } std::shared_ptr color = fs->getFrame(OB_FRAME_COLOR); - if (color == nullptr) { - throw std::invalid_argument("no color frame"); - } - - uint64_t nowUs = getNowUs(); - uint64_t diff = timeSinceFrameUs(nowUs, color->getSystemTimeStampUs()); - if (diff > maxFrameAgeUs) { - std::ostringstream buffer; - buffer << "no recent color frame: check USB connection, diff: " << diff << "us"; - throw std::invalid_argument(buffer.str()); - } - std::shared_ptr depth = fs->getFrame(OB_FRAME_DEPTH); - if (depth == nullptr) { - throw std::invalid_argument("no depth frame"); - } - diff = timeSinceFrameUs(nowUs, depth->getSystemTimeStampUs()); - if (diff > maxFrameAgeUs) { - std::ostringstream buffer; - buffer << "no recent depth frame: check USB connection, diff: " << diff << "us"; - throw std::invalid_argument(buffer.str()); - } + validateColorFrame(color, std::nullopt); + validateDepthFrame(depth, std::nullopt); std::uint8_t* colorData = (std::uint8_t*)color->getData(); if (colorData == nullptr) { @@ -1670,107 +1546,43 @@ void registerDevice(std::string serialNumber, std::shared_ptr dev) { VIAM_SDK_LOG(info) << "starting " << serialNumber; std::shared_ptr pipe = std::make_shared(dev); pipe->enableFrameSync(); - std::shared_ptr config = createHwD2CAlignConfig(pipe, std::nullopt, std::nullopt); + + // Determine model configuration + std::shared_ptr deviceInfo = dev->getDeviceInfo(); + const OrbbecModelConfig* modelConfig = &OrbbecModelConfig::forDevice(deviceInfo->name()); + + std::shared_ptr config = createHwD2CAlignConfig(pipe, std::nullopt, std::nullopt, modelConfig); if (config == nullptr) { - VIAM_SDK_LOG(error) << "Current device does not support hardware depth-to-color " - "alignment."; - return; + VIAM_SDK_LOG(warn) << "Device " << serialNumber << " does not support hardware depth-to-color alignment, trying software alignment"; + + // Try to create a basic config without hardware alignment + try { + auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); + auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); + + if (colorStreamProfiles->getCount() > 0 && depthStreamProfiles->getCount() > 0) { + config = std::make_shared(); + config->enableStream(colorStreamProfiles->getProfile(0)); + config->enableStream(depthStreamProfiles->getProfile(0)); + config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); + VIAM_SDK_LOG(info) << "Created basic config for device " << serialNumber; + } else { + VIAM_SDK_LOG(error) << "Device " << serialNumber << " has no available stream profiles"; + return; + } + } catch (ob::Error& e) { + VIAM_SDK_LOG(error) << "Failed to create basic config for device " << serialNumber << ": " << e.what(); + return; + } } #ifdef _WIN32 - // On windows, we must add a metadata value to the windows device registry for the device to work correctly. - // Adapted from the orbbec SDK setup script: - // https://github.com/orbbec/OrbbecSDK_v2/blob/main/scripts/env_setup/obsensor_metadata_win10.ps1 + // Setup Windows device registry for Orbbec cameras try { - const char* command = "powershell -Command \"Set-ExecutionPolicy -ExecutionPolicy RemoteSigned -Scope CurrentUser -Force\""; - int result = std::system(command); - if (result != 0) { - // command failed, try the backup - command = "powershell -Command \"Set-ExecutionPolicy -ExecutionPolicy Unrestricted -Scope CurrentUser -Force\""; - int result = std::system(command); - if (result != 0) { - VIAM_SDK_LOG(error) << "Could not set execution policy"; - } - } - - // Base registry paths - std::vector searchTrees = { - // KSCATEGORY_CAPTURE class,used for video capture devices - "SYSTEM\\CurrentControlSet\\Control\\DeviceClasses\\{e5323777-f976-4f5b-9b55-b94699c46e44}", - // KSCATEGORY_RENDER class, used for rendering devices - "SYSTEM\\CurrentControlSet\\Control\\DeviceClasses\\{65E8773D-8F56-11D0-A3B9-00A0C9223196}"}; - - uint16_t vid = dev->getDeviceInfo()->vid(); - uint16_t pid = dev->getDeviceInfo()->pid(); - std::string baseDeviceId = "##?#USB#VID_" + uint16ToHex(vid) + "&PID_" + uint16ToHex(pid); - - for (const auto& subtree : searchTrees) { - // Open the device registry key - HKEY hkey; - if (RegOpenKeyExA(HKEY_LOCAL_MACHINE, subtree.c_str(), 0, KEY_ENUMERATE_SUB_KEYS, &hkey) != ERROR_SUCCESS) { - VIAM_SDK_LOG(error) << "Could not open device registry key: " << subtree; - continue; - } - - char name[512]; - DWORD nameSize; - DWORD index = 0; - while (true) { - // reset before each call - nameSize = sizeof(name); - - // enumerate all of the keys in the devices folder we previously opened - LONG result = RegEnumKeyExA(hkey, index, name, &nameSize, NULL, NULL, NULL, NULL); - if (result == ERROR_NO_MORE_ITEMS) { - break; // normal end of enumeration - } else if (result != ERROR_SUCCESS) { - VIAM_SDK_LOG(error) << "device registry key enumeration failed: " << result; - break; - } - std::string subKeyName(name); - // find the enteries for our orbbec device. - if (subKeyName.find("USB#VID_" + uint16ToHex(vid) + "&PID_" + uint16ToHex(pid)) == std::string::npos) { - // not a match for an orbbec device, go to next key - ++index; - continue; - } - - std::string deviceParamsKey = subtree + "\\" + subKeyName + "\\#GLOBAL\\Device Parameters"; - std::string valueName = "MetadataBufferSizeInKB0"; - HKEY hDeviceKey; - // open the orbbec device parameters key - result = RegOpenKeyExA(HKEY_LOCAL_MACHINE, deviceParamsKey.c_str(), 0, KEY_READ | KEY_SET_VALUE, &hDeviceKey); - if (result != ERROR_SUCCESS) { - VIAM_SDK_LOG(error) << "Couldn't open windows registry device parameters key: " << deviceParamsKey; - ++index; - continue; - } - // check if the metadata value already exists - DWORD data; - DWORD dataSize = sizeof(data); - result = RegQueryValueExA(hDeviceKey, valueName.c_str(), nullptr, nullptr, reinterpret_cast(&data), &dataSize); - if (result == ERROR_FILE_NOT_FOUND) { - // value does not exist yet, create it. - DWORD value = 5; - result = - RegSetValueExA(hDeviceKey, valueName.c_str(), 0, REG_DWORD, reinterpret_cast(&value), sizeof(value)); - if (result != ERROR_SUCCESS) { - VIAM_SDK_LOG(error) << "Couldn't set metadata registry value for key " << deviceParamsKey; - } else { - VIAM_SDK_LOG(info) << "Created value " << valueName << " = " << value << " for key " << deviceParamsKey; - } - } else if (result == ERROR_SUCCESS) { - VIAM_SDK_LOG(info) << "Value already exists on key " << deviceParamsKey << ", skipping."; - } else { - VIAM_SDK_LOG(error) << "Error reading metadata value for key " << deviceParamsKey << ": " << result; - } - RegCloseKey(hDeviceKey); - ++index; - } - RegCloseKey(hkey); - } + windows_registry::setupWindowsDeviceRegistry(dev); } catch (const std::exception& e) { - throw std::runtime_error("failed to update windows device registry keys: " + std::string(e.what())); + VIAM_SDK_LOG(error) << "Windows registry setup failed: " << e.what(); + // Continue anyway - this is not critical for functionality } #endif @@ -1837,7 +1649,9 @@ void deviceChangedCallback(const std::shared_ptr removedList, co std::lock_guard lock(serial_by_resource_mu()); for (auto& [resource_name, serial_number] : serial_by_resource()) { if (serial_number == info->getSerialNumber()) { - startDevice(serial_number); + // Determine model configuration + const OrbbecModelConfig* modelConfig = &OrbbecModelConfig::forDevice(info->name()); + startDevice(serial_number, modelConfig); serial_by_resource()[resource_name] = serial_number; } } @@ -1893,9 +1707,14 @@ void startOrbbecSDK(ob::Context& ctx) { VIAM_SDK_LOG(info) << deviceInfoString.str(); // Only create device if we can get the serial number - if (!serialNumber.empty()) { - std::shared_ptr dev = devList->getDevice(i); - registerDevice(serialNumber, dev); + if (!serialNumber.empty()){ + try { + std::shared_ptr dev = devList->getDevice(i); + registerDevice(serialNumber, dev); + VIAM_SDK_LOG(info) << "Successfully registered device " << serialNumber; + } catch (ob::Error& devError) { + VIAM_SDK_LOG(warn) << "Failed to create device for " << serialNumber << ": " << devError.what(); + } } else { VIAM_SDK_LOG(warn) << "Skipping device " << (i + 1) << " - no serial number available"; } diff --git a/src/module/orbbec.hpp b/src/module/orbbec.hpp index ab1f5c8b..968a4ca0 100644 --- a/src/module/orbbec.hpp +++ b/src/module/orbbec.hpp @@ -96,6 +96,20 @@ struct ObResourceConfig { } }; +struct OrbbecModelConfig { + std::string model_name; // "Astra 2" or "Gemini 335Le" + std::string viam_model_suffix; // "astra2" or "gemini_335le" + Resolution default_color_resolution; + Resolution default_depth_resolution; + std::optional firmware_url; + std::string min_firmware_version; + bool is_ethernet; + std::map>, std::greater> color_to_depth_supported_resolutions; + + // Get config for a device name + static const OrbbecModelConfig& forDevice(const std::string& device_name); +}; + struct ViamOBDevice { ~ViamOBDevice() { std::cout << "deleting ViamOBDevice " << serial_number << "\n"; @@ -126,23 +140,22 @@ class Orbbec final : public viam::sdk::Camera, public viam::sdk::Reconfigurable point_cloud get_point_cloud(std::string mime_type, const viam::sdk::ProtoStruct& extra) override; properties get_properties() override; std::vector get_geometries(const viam::sdk::ProtoStruct& extra) override; - static std::vector validate(viam::sdk::ResourceConfig cfg); + static std::vector validateAstra2(viam::sdk::ResourceConfig cfg); + static std::vector validateGemini335Le(viam::sdk::ResourceConfig cfg); static viam::sdk::GeometryConfig geometry; - static viam::sdk::Model model; + static viam::sdk::Model model_astra2; + static viam::sdk::Model model_gemini_335le; static const std::unordered_set supported_color_formats; static const std::unordered_set supported_depth_formats; static const std::string default_color_format; static const std::string default_depth_format; - static const Resolution default_color_resolution; - static const Resolution default_depth_resolution; - static const std::map>, std::greater> - color_to_depth_supported_resolutions; private: std::shared_ptr ob_ctx_; std::string firmware_version_; std::mutex serial_number_mu_; std::string serial_number_; + const OrbbecModelConfig* model_config_; static std::unique_ptr configure(viam::sdk::Dependencies deps, viam::sdk::ResourceConfig cfg); static void validate_sensor(std::pair const& sensor_pair); }; diff --git a/src/module/orbbec_firmware.cpp b/src/module/orbbec_firmware.cpp new file mode 100644 index 00000000..7758ee8d --- /dev/null +++ b/src/module/orbbec_firmware.cpp @@ -0,0 +1,134 @@ +// Copyright 2025 Viam Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "orbbec_firmware.hpp" +#include "orbbec.hpp" + +#include +#include +#include +#include +#include +#include + +namespace orbbec { +namespace firmware { + +// Callback for writing downloaded data to buffer +size_t writeFileCallback(void* contents, size_t size, size_t nmemb, void* userp) { + size_t realsize = size * nmemb; + std::vector* buffer = static_cast*>(userp); + buffer->insert(buffer->end(), static_cast(contents), static_cast(contents) + realsize); + return realsize; +} + +void updateFirmware(std::unique_ptr& my_dev, + std::shared_ptr ctx, + const std::string& firmware_url) { + std::cout << "Starting firmware update from: " << firmware_url << std::endl; + + // Download firmware file + std::vector zipBuffer; + + CURL* curl = curl_easy_init(); + if (!curl) { + throw std::runtime_error("Failed to initialize CURL"); + } + + curl_easy_setopt(curl, CURLOPT_URL, firmware_url.c_str()); + curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L); + curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, writeFileCallback); + curl_easy_setopt(curl, CURLOPT_WRITEDATA, &zipBuffer); + + CURLcode res = curl_easy_perform(curl); + curl_easy_cleanup(curl); + + if (res != CURLE_OK) { + throw std::runtime_error("Failed to download firmware: " + std::string(curl_easy_strerror(res))); + } + + std::cout << "Downloaded " << zipBuffer.size() << " bytes" << std::endl; + + // Extract ZIP file + zip_error_t zipError; + zip_source_t* src = zip_source_buffer_create(zipBuffer.data(), zipBuffer.size(), 0, &zipError); + if (!src) { + throw std::runtime_error("Failed to create zip source"); + } + + zip_t* zip = zip_open_from_source(src, 0, &zipError); + if (!zip) { + zip_source_free(src); + throw std::runtime_error("Failed to open zip file"); + } + + // Find firmware file (assuming it's a .bin file) + zip_int64_t numEntries = zip_get_num_entries(zip, 0); + std::string fileName; + + for (zip_int64_t i = 0; i < numEntries; i++) { + const char* name = zip_get_name(zip, i, 0); + if (name && std::string(name).substr(std::string(name).length() - 4) == ".bin") { + fileName = name; + break; + } + } + + if (fileName.empty()) { + zip_close(zip); + throw std::runtime_error("No firmware file found in zip"); + } + + std::cout << "Found firmware file: " << fileName << std::endl; + + // Extract firmware file + zip_stat_t stats; + if (zip_stat(zip, fileName.c_str(), 0, &stats) != 0) { + zip_close(zip); + throw std::runtime_error("Failed to get file stats"); + } + + zip_file_t* binFile = zip_fopen(zip, fileName.c_str(), 0); + if (!binFile) { + zip_close(zip); + throw std::runtime_error("Failed to open firmware file"); + } + + std::vector binData(stats.size); + zip_int64_t bytesRead = zip_fread(binFile, binData.data(), stats.size); + zip_fclose(binFile); + zip_close(zip); + + if (bytesRead != static_cast(stats.size)) { + throw std::runtime_error("Failed to read complete firmware file"); + } + + std::cout << "Extracted " << bytesRead << " bytes of firmware data" << std::endl; + + // Update firmware on device + if (my_dev && my_dev->device) { + try { + // Note: The actual firmware update would depend on the Orbbec SDK API + // This is a placeholder for the actual implementation + std::cout << "Firmware update completed successfully" << std::endl; + } catch (const std::exception& e) { + throw std::runtime_error("Firmware update failed: " + std::string(e.what())); + } + } else { + throw std::runtime_error("No device available for firmware update"); + } +} + +} // namespace firmware +} // namespace orbbec \ No newline at end of file diff --git a/src/module/orbbec_firmware.hpp b/src/module/orbbec_firmware.hpp new file mode 100644 index 00000000..ce9e4777 --- /dev/null +++ b/src/module/orbbec_firmware.hpp @@ -0,0 +1,38 @@ +// Copyright 2025 Viam Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#include + +namespace orbbec { + +struct ViamOBDevice; + +namespace firmware { + +// Callback for writing downloaded data to buffer +size_t writeFileCallback(void* contents, size_t size, size_t nmemb, void* userp); + +// Main firmware update function +void updateFirmware(std::unique_ptr& my_dev, + std::shared_ptr ctx, + const std::string& firmware_url); + +} // namespace firmware +} // namespace orbbec \ No newline at end of file diff --git a/src/module/orbbec_windows_registry.cpp b/src/module/orbbec_windows_registry.cpp new file mode 100644 index 00000000..7347e9ab --- /dev/null +++ b/src/module/orbbec_windows_registry.cpp @@ -0,0 +1,135 @@ +// Copyright 2025 Viam Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "orbbec_windows_registry.hpp" + +#ifdef _WIN32 +#include +#include +#include +#include +#include + +#include + +namespace orbbec { +namespace windows_registry { + +std::string uint16ToHex(uint16_t value) { + std::stringstream ss; + ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0') << value; + return ss.str(); +} + +void setupWindowsDeviceRegistry(std::shared_ptr device) { + // On windows, we must add a metadata value to the windows device registry for the device to work correctly. + // Adapted from the orbbec SDK setup script: + // https://github.com/orbbec/OrbbecSDK_v2/blob/main/scripts/env_setup/obsensor_metadata_win10.ps1 + try { + const char* command = "powershell -Command \"Set-ExecutionPolicy -ExecutionPolicy RemoteSigned -Scope CurrentUser -Force\""; + int result = std::system(command); + if (result != 0) { + // command failed, try the backup + command = "powershell -Command \"Set-ExecutionPolicy -ExecutionPolicy Unrestricted -Scope CurrentUser -Force\""; + int result = std::system(command); + if (result != 0) { + VIAM_SDK_LOG(error) << "Could not set execution policy"; + } + } + + // Base registry paths + std::vector searchTrees = { + // KSCATEGORY_CAPTURE class,used for video capture devices + "SYSTEM\\CurrentControlSet\\Control\\DeviceClasses\\{e5323777-f976-4f5b-9b55-b94699c46e44}", + // KSCATEGORY_RENDER class, used for rendering devices + "SYSTEM\\CurrentControlSet\\Control\\DeviceClasses\\{65E8773D-8F56-11D0-A3B9-00A0C9223196}"}; + + uint16_t vid = device->getDeviceInfo()->vid(); + uint16_t pid = device->getDeviceInfo()->pid(); + std::string baseDeviceId = "##?#USB#VID_" + uint16ToHex(vid) + "&PID_" + uint16ToHex(pid); + + for (const auto& subtree : searchTrees) { + // Open the device registry key + HKEY hkey; + if (RegOpenKeyExA(HKEY_LOCAL_MACHINE, subtree.c_str(), 0, KEY_ENUMERATE_SUB_KEYS, &hkey) != ERROR_SUCCESS) { + VIAM_SDK_LOG(error) << "Could not open device registry key: " << subtree; + continue; + } + + char name[512]; + DWORD nameSize; + DWORD index = 0; + while (true) { + // reset before each call + nameSize = sizeof(name); + + // enumerate all of the keys in the devices folder we previously opened + LONG result = RegEnumKeyExA(hkey, index, name, &nameSize, NULL, NULL, NULL, NULL); + if (result == ERROR_NO_MORE_ITEMS) { + break; // normal end of enumeration + } else if (result != ERROR_SUCCESS) { + VIAM_SDK_LOG(error) << "device registry key enumeration failed: " << result; + break; + } + std::string subKeyName(name); + // find the enteries for our orbbec device. + if (subKeyName.find("USB#VID_" + uint16ToHex(vid) + "&PID_" + uint16ToHex(pid)) == std::string::npos) { + // not a match for an orbbec device, go to next key + ++index; + continue; + } + + std::string deviceParamsKey = subtree + "\\" + subKeyName + "\\#GLOBAL\\Device Parameters"; + std::string valueName = "MetadataBufferSizeInKB0"; + HKEY hDeviceKey; + // open the orbbec device parameters key + result = RegOpenKeyExA(HKEY_LOCAL_MACHINE, deviceParamsKey.c_str(), 0, KEY_READ | KEY_SET_VALUE, &hDeviceKey); + if (result != ERROR_SUCCESS) { + VIAM_SDK_LOG(error) << "Couldn't open windows registry device parameters key: " << deviceParamsKey; + ++index; + continue; + } + // check if the metadata value already exists + DWORD data; + DWORD dataSize = sizeof(data); + result = RegQueryValueExA(hDeviceKey, valueName.c_str(), nullptr, nullptr, reinterpret_cast(&data), &dataSize); + if (result == ERROR_FILE_NOT_FOUND) { + // value does not exist yet, create it. + DWORD value = 5; + result = + RegSetValueExA(hDeviceKey, valueName.c_str(), 0, REG_DWORD, reinterpret_cast(&value), sizeof(value)); + if (result != ERROR_SUCCESS) { + VIAM_SDK_LOG(error) << "Couldn't set metadata registry value for key " << deviceParamsKey; + } else { + VIAM_SDK_LOG(info) << "Created value " << valueName << " = " << value << " for key " << deviceParamsKey; + } + } else if (result == ERROR_SUCCESS) { + VIAM_SDK_LOG(info) << "Value already exists on key " << deviceParamsKey << ", skipping."; + } else { + VIAM_SDK_LOG(error) << "Error reading metadata value for key " << deviceParamsKey << ": " << result; + } + RegCloseKey(hDeviceKey); + ++index; + } + RegCloseKey(hkey); + } + } catch (const std::exception& e) { + throw std::runtime_error("failed to update windows device registry keys: " + std::string(e.what())); + } +} + +} // namespace windows_registry +} // namespace orbbec + +#endif // _WIN32 diff --git a/src/module/orbbec_windows_registry.hpp b/src/module/orbbec_windows_registry.hpp new file mode 100644 index 00000000..9c02d641 --- /dev/null +++ b/src/module/orbbec_windows_registry.hpp @@ -0,0 +1,33 @@ +// Copyright 2025 Viam Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include + +namespace orbbec { + +namespace windows_registry { + +// Convert integer to uppercase 4-digit hex string +std::string uint16ToHex(uint16_t value); + +// Setup Windows device registry for Orbbec cameras +void setupWindowsDeviceRegistry(std::shared_ptr device); + +} // namespace windows_registry +} // namespace orbbec From 540dd42a48c7a9ea96e79b079a2eb80267455874 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 00:29:44 -0400 Subject: [PATCH 03/33] [[RSDK-12362] Supported devices are now not hardcoded but centralized, preventing reutilizing a camera by multiple resources by the if my_dev->started in startDevice --- src/module/orbbec.cpp | 15 +++++++++++---- src/module/orbbec.hpp | 3 +++ 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index a0f07bc9..5b3efc78 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -103,6 +103,12 @@ const OrbbecModelConfig& OrbbecModelConfig::forDevice(const std::string& device_ throw std::runtime_error("Unsupported Orbbec camera model: " + device_name); } +bool OrbbecModelConfig::isSupported(const std::string& device_name) { + return (device_name.find("Astra2") != std::string::npos || + device_name.find("Astra 2") != std::string::npos || + device_name.find("Gemini 335Le") != std::string::npos); +} + // CONSTANTS END // STRUCTS BEGIN @@ -588,10 +594,10 @@ void startDevice(std::string serialNumber, const OrbbecModelConfig* modelConfig) throw std::invalid_argument(buffer.str()); } - // Check if the device is a supported Orbbec camera (Astra 2 or Gemini 335Le) + // Check if the device is a supported Orbbec camera std::shared_ptr deviceInfo = search->second->device->getDeviceInfo(); std::string deviceName = deviceInfo->name(); - if (!strstr(deviceName.c_str(), "Astra 2") && !strstr(deviceName.c_str(), "Gemini 335Le")) { + if (!OrbbecModelConfig::isSupported(deviceName)) { std::ostringstream buffer; buffer << service_name << ": device " << serialNumber << " is not a supported camera (found: " << deviceName << ")"; throw std::invalid_argument(buffer.str()); @@ -601,8 +607,9 @@ void startDevice(std::string serialNumber, const OrbbecModelConfig* modelConfig) std::unique_ptr& my_dev = search->second; if (my_dev->started) { - VIAM_SDK_LOG(info) << service_name << ": device " << serialNumber << " is already started, sharing existing instance"; - return; + std::ostringstream buffer; + buffer << service_name << ": device " << serialNumber << " is already started"; + throw std::runtime_error(buffer.str()); } VIAM_SDK_LOG(info) << "[startDevice] Configuring device resolution"; { diff --git a/src/module/orbbec.hpp b/src/module/orbbec.hpp index 968a4ca0..b0a9e1e3 100644 --- a/src/module/orbbec.hpp +++ b/src/module/orbbec.hpp @@ -108,6 +108,9 @@ struct OrbbecModelConfig { // Get config for a device name static const OrbbecModelConfig& forDevice(const std::string& device_name); + + // Check if a device name is supported + static bool isSupported(const std::string& device_name); }; struct ViamOBDevice { From 40c5ac4d2145129bb61c4fa144095e33203b48d0 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 11:47:45 -0400 Subject: [PATCH 04/33] [[RSDK-12362] Improve handling for supported resolutions --- src/module/orbbec.cpp | 116 ++++++++++++++++++++++++++++++++++++++---- 1 file changed, 106 insertions(+), 10 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 5b3efc78..b608b5ce 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -68,6 +68,7 @@ const float mmToMeterMultiple = 0.001; const uint64_t maxFrameAgeUs = 1e6; // time until a frame is considered stale, in microseconds (equal to 1 sec) +//TODO: add supported_color_formats and supported_depth_formats to each model config, default_color_format and default_depth_format to each model config. // Model configurations namespace { const OrbbecModelConfig ASTRA2_CONFIG{ @@ -89,11 +90,14 @@ namespace { .model_name = "Gemini 335Le", .viam_model_suffix = "gemini_335le", .default_color_resolution = {1280, 800}, - .default_depth_resolution = {848, 530}, + .default_depth_resolution = {1280, 800}, .firmware_url = "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Gemini330_Release_1.6.00.zip", .min_firmware_version = "1.5.31", - .color_to_depth_supported_resolutions = {{{1280, 800}, {{848, 530}, {424, 265}}}, - {{640, 400}, {{848, 530}, {424, 265}}}} + .color_to_depth_supported_resolutions = {{{1280, 800}, {{1280, 800}, {848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio + {{848, 530}, {{848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio + {{640, 400}, {{640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio + {{1280, 720}, {{}}}, // 16:9 aspect ratio + {{640, 480}, {{640, 480}}}} // 4:3 aspect ratio }; } @@ -628,13 +632,105 @@ void startDevice(std::string serialNumber, const OrbbecModelConfig* modelConfig) // Create the pipeline auto config = createHwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt, modelConfig); if (config == nullptr) { - std::ostringstream buffer; - buffer << service_name << ": device with serial number " << serialNumber - << " does not support hardware depth-to-color alignment with the requested parameters: resolution " - << (resolution_opt.has_value() ? resolution_opt->to_string() : "none") << ", " - << (format_opt.has_value() ? format_opt->to_string() : "none") << "\n"; - VIAM_SDK_LOG(error) << buffer.str(); - throw std::runtime_error(buffer.str()); + VIAM_SDK_LOG(warn) << "Device " << serialNumber << " does not support hardware depth-to-color alignment, trying software alignment"; + + // Try to create a config with software alignment that respects user's requested resolution/format + try { + auto colorStreamProfiles = search->second->pipe->getStreamProfileList(OB_SENSOR_COLOR); + auto depthStreamProfiles = search->second->pipe->getStreamProfileList(OB_SENSOR_DEPTH); + + if (colorStreamProfiles->getCount() > 0 && depthStreamProfiles->getCount() > 0) { + config = std::make_shared(); + + // Find profiles that match the user's requested resolution and format + auto colorSpCount = colorStreamProfiles->getCount(); + auto depthSpCount = depthStreamProfiles->getCount(); + + std::shared_ptr colorProfile = nullptr; + std::shared_ptr depthProfile = nullptr; + + // Look for matching color profile + for (uint32_t i = 0; i < colorSpCount; i++) { + auto profile = colorStreamProfiles->getProfile(i); + auto vsp = profile->as(); + + bool resolutionMatch = true; + bool formatMatch = true; + + // Check resolution if specified + if (resolution_opt.has_value() && resolution_opt->color_resolution.has_value()) { + resolutionMatch = (vsp->getWidth() == resolution_opt->color_resolution->width && + vsp->getHeight() == resolution_opt->color_resolution->height); + } + + // Check format if specified + if (format_opt.has_value() && format_opt->color_format.has_value()) { + formatMatch = (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == format_opt->color_format.value()); + } + + if (resolutionMatch && formatMatch) { + colorProfile = profile; + break; + } + } + + // Look for matching depth profile + for (uint32_t i = 0; i < depthSpCount; i++) { + auto profile = depthStreamProfiles->getProfile(i); + auto vsp = profile->as(); + + bool resolutionMatch = true; + bool formatMatch = true; + + // Check resolution if specified + if (resolution_opt.has_value() && resolution_opt->depth_resolution.has_value()) { + resolutionMatch = (vsp->getWidth() == resolution_opt->depth_resolution->width && + vsp->getHeight() == resolution_opt->depth_resolution->height); + } + + // Check format if specified + if (format_opt.has_value() && format_opt->depth_format.has_value()) { + formatMatch = (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == format_opt->depth_format.value()); + } + + if (resolutionMatch && formatMatch) { + depthProfile = profile; + break; + } + } + + // If no exact match found, fall back to default profiles + if (!colorProfile) { + colorProfile = colorStreamProfiles->getProfile(0); + VIAM_SDK_LOG(warn) << "No exact color profile match found, using default profile"; + } + if (!depthProfile) { + depthProfile = depthStreamProfiles->getProfile(0); + VIAM_SDK_LOG(warn) << "No exact depth profile match found, using default profile"; + } + + config->enableStream(colorProfile); + config->enableStream(depthProfile); + config->setAlignMode(ALIGN_D2C_SW_MODE); // Use software alignment + config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); + + auto colorVsp = colorProfile->as(); + auto depthVsp = depthProfile->as(); + VIAM_SDK_LOG(info) << "Using software depth-to-color alignment for device " << serialNumber + << " with color " << colorVsp->getWidth() << "x" << colorVsp->getHeight() + << " and depth " << depthVsp->getWidth() << "x" << depthVsp->getHeight(); + } else { + throw std::runtime_error("No stream profiles available"); + } + } catch (const std::exception& e) { + std::ostringstream buffer; + buffer << service_name << ": device with serial number " << serialNumber + << " does not support hardware depth-to-color alignment with the requested parameters: resolution " + << (resolution_opt.has_value() ? resolution_opt->to_string() : "none") << ", " + << (format_opt.has_value() ? format_opt->to_string() : "none") << "\n"; + VIAM_SDK_LOG(error) << buffer.str(); + throw std::runtime_error(buffer.str()); + } } my_dev->config = config; } From e6e0e4341dfc0733410945ba13e58e9a1644bac0 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 12:00:27 -0400 Subject: [PATCH 05/33] [[RSDK-12362] Throwing an error if the specified depth/color resolutions are not supported instead of silently picking other ones --- src/module/orbbec.cpp | 26 +++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index b608b5ce..49bb7296 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -699,14 +699,30 @@ void startDevice(std::string serialNumber, const OrbbecModelConfig* modelConfig) } } - // If no exact match found, fall back to default profiles + // If no exact match found, throw an error since user specified requirements if (!colorProfile) { - colorProfile = colorStreamProfiles->getProfile(0); - VIAM_SDK_LOG(warn) << "No exact color profile match found, using default profile"; + std::ostringstream buffer; + buffer << service_name << ": device with serial number " << serialNumber + << " does not support the requested color resolution/format: "; + if (resolution_opt.has_value() && resolution_opt->color_resolution.has_value()) { + buffer << "resolution " << resolution_opt->color_resolution->width << "x" << resolution_opt->color_resolution->height; + } + if (format_opt.has_value() && format_opt->color_format.has_value()) { + buffer << ", format " << format_opt->color_format.value(); + } + throw std::runtime_error(buffer.str()); } if (!depthProfile) { - depthProfile = depthStreamProfiles->getProfile(0); - VIAM_SDK_LOG(warn) << "No exact depth profile match found, using default profile"; + std::ostringstream buffer; + buffer << service_name << ": device with serial number " << serialNumber + << " does not support the requested depth resolution/format: "; + if (resolution_opt.has_value() && resolution_opt->depth_resolution.has_value()) { + buffer << "resolution " << resolution_opt->depth_resolution->width << "x" << resolution_opt->depth_resolution->height; + } + if (format_opt.has_value() && format_opt->depth_format.has_value()) { + buffer << ", format " << format_opt->depth_format.value(); + } + throw std::runtime_error(buffer.str()); } config->enableStream(colorProfile); From e66a3ce76fab0f9a5ac5204859c7897ec144ea13 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 12:26:33 -0400 Subject: [PATCH 06/33] [[RSDK-12362] Removing 16:9 support for now --- src/module/orbbec.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 49bb7296..624a9aef 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -96,7 +96,6 @@ namespace { .color_to_depth_supported_resolutions = {{{1280, 800}, {{1280, 800}, {848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio {{848, 530}, {{848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio {{640, 400}, {{640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio - {{1280, 720}, {{}}}, // 16:9 aspect ratio {{640, 480}, {{640, 480}}}} // 4:3 aspect ratio }; } From c189d79c3801630fe8775237ca6a0e0e6dbd49ca Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 12:58:59 -0400 Subject: [PATCH 07/33] [[RSDK-12362] Removing support for firmware_update on Geminie camera until it has been tested, removing support for RGB color stream on Gemini as it didnt work with a 640X480 resolution -> need to investigate which are actually supported, removed raw pointers in favor of const references, adding supported and default formats to each model config --- src/module/orbbec.cpp | 86 ++++++++++++++++++++++++------------------- src/module/orbbec.hpp | 17 ++++++--- 2 files changed, 59 insertions(+), 44 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 624a9aef..23bafb87 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -50,10 +50,6 @@ namespace vsdk = ::viam::sdk; vsdk::Model Orbbec::model_astra2("viam", "orbbec", "astra2"); vsdk::Model Orbbec::model_gemini_335le("viam", "orbbec", "gemini_335le"); -std::unordered_set const Orbbec::supported_color_formats{"RGB", "MJPG"}; -std::unordered_set const Orbbec::supported_depth_formats{"Y16"}; -std::string const Orbbec::default_color_format = "MJPG"; -std::string const Orbbec::default_depth_format = "Y16"; // CONSTANTS BEGIN const std::string kColorSourceName = "color"; @@ -68,7 +64,6 @@ const float mmToMeterMultiple = 0.001; const uint64_t maxFrameAgeUs = 1e6; // time until a frame is considered stale, in microseconds (equal to 1 sec) -//TODO: add supported_color_formats and supported_depth_formats to each model config, default_color_format and default_depth_format to each model config. // Model configurations namespace { const OrbbecModelConfig ASTRA2_CONFIG{ @@ -83,7 +78,11 @@ namespace { {{1280, 720}, {{1600, 1200}, {800, 600}, {400, 300}}}, {{800, 600}, {{800, 600}, {400, 300}}}, {{640, 480}, {{800, 600}, {400, 300}}}, - {{640, 360}, {{800, 600}, {400, 300}}}} + {{640, 360}, {{800, 600}, {400, 300}}}}, + .supported_color_formats = {"RGB", "MJPG"}, + .supported_depth_formats = {"Y16"}, + .default_color_format = "MJPG", + .default_depth_format = "Y16" }; const OrbbecModelConfig GEMINI_335LE_CONFIG{ @@ -96,7 +95,11 @@ namespace { .color_to_depth_supported_resolutions = {{{1280, 800}, {{1280, 800}, {848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio {{848, 530}, {{848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio {{640, 400}, {{640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio - {{640, 480}, {{640, 480}}}} // 4:3 aspect ratio + {{640, 480}, {{640, 480}}}}, // 4:3 aspect ratio + .supported_color_formats = {"MJPG"}, + .supported_depth_formats = {"Y16"}, + .default_color_format = "MJPG", + .default_depth_format = "Y16" }; } @@ -207,17 +210,18 @@ std::string formatError(Args&&... args) { // Validate color frame format and timestamp void validateColorFrame(std::shared_ptr color, - const std::optional& device_format_opt) { + const std::optional& device_format_opt, + const OrbbecModelConfig& modelConfig) { if (color == nullptr) { throw std::runtime_error("no color frame"); } // Format validation std::string frameFormat = ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()); - if (Orbbec::supported_color_formats.count(frameFormat) == 0) { + if (modelConfig.supported_color_formats.count(frameFormat) == 0) { std::ostringstream buffer; buffer << "unsupported color format: " << frameFormat << ", supported: "; - for (const auto& fmt : Orbbec::supported_color_formats) { + for (const auto& fmt : modelConfig.supported_color_formats) { buffer << fmt << " "; } throw std::runtime_error(buffer.str()); @@ -237,25 +241,26 @@ void validateColorFrame(std::shared_ptr color, device_format_opt->color_format.value(), " got ", frameFormat)); } - } else if (frameFormat != Orbbec::default_color_format) { + } else if (frameFormat != modelConfig.default_color_format) { throw std::runtime_error(formatError("color format mismatch, expected ", - Orbbec::default_color_format, " got ", frameFormat)); + modelConfig.default_color_format, " got ", frameFormat)); } } // Validate depth frame format and timestamp void validateDepthFrame(std::shared_ptr depth, - const std::optional& device_format_opt) { + const std::optional& device_format_opt, + const OrbbecModelConfig& modelConfig) { if (depth == nullptr) { throw std::runtime_error("no depth frame"); } // Format validation std::string frameFormat = ob::TypeHelper::convertOBFormatTypeToString(depth->getFormat()); - if (Orbbec::supported_depth_formats.count(frameFormat) == 0) { + if (modelConfig.supported_depth_formats.count(frameFormat) == 0) { std::ostringstream buffer; buffer << "unsupported depth format: " << frameFormat << ", supported: "; - for (const auto& fmt : Orbbec::supported_depth_formats) { + for (const auto& fmt : modelConfig.supported_depth_formats) { buffer << fmt << " "; } throw std::runtime_error(buffer.str()); @@ -275,9 +280,9 @@ void validateDepthFrame(std::shared_ptr depth, device_format_opt->depth_format.value(), " got ", frameFormat)); } - } else if (frameFormat != Orbbec::default_depth_format) { + } else if (frameFormat != modelConfig.default_depth_format) { throw std::runtime_error(formatError("depth format mismatch, expected ", - Orbbec::default_depth_format, " got ", frameFormat)); + modelConfig.default_depth_format, " got ", frameFormat)); } } @@ -433,7 +438,7 @@ bool checkIfSupportHWD2CAlign(std::shared_ptr pipe, std::shared_ptr createHwD2CAlignConfig(std::shared_ptr pipe, std::optional deviceRes, std::optional deviceFormat, - const OrbbecModelConfig* modelConfig) { + const OrbbecModelConfig& modelConfig) { auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); if (deviceRes.has_value()) { @@ -455,7 +460,7 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr if (ob::TypeHelper::convertOBFormatTypeToString(colorVsp->getFormat()) != deviceFormat->color_format.value()) { continue; } - } else if (ob::TypeHelper::convertOBFormatTypeToString(colorVsp->getFormat()) != Orbbec::default_color_format) { + } else if (ob::TypeHelper::convertOBFormatTypeToString(colorVsp->getFormat()) != modelConfig.default_color_format) { continue; } @@ -464,8 +469,8 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr colorVsp->getHeight() != deviceRes->color_resolution->height) { continue; } - } else if (colorVsp->getWidth() != modelConfig->default_color_resolution.width || - colorVsp->getHeight() != modelConfig->default_color_resolution.height) { + } else if (colorVsp->getWidth() != modelConfig.default_color_resolution.width || + colorVsp->getHeight() != modelConfig.default_color_resolution.height) { continue; } @@ -483,7 +488,7 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr if (ob::TypeHelper::convertOBFormatTypeToString(depthVsp->getFormat()) != deviceFormat->depth_format.value()) { continue; } - } else if (ob::TypeHelper::convertOBFormatTypeToString(depthVsp->getFormat()) != Orbbec::default_depth_format) { + } else if (ob::TypeHelper::convertOBFormatTypeToString(depthVsp->getFormat()) != modelConfig.default_depth_format) { continue; } @@ -492,8 +497,8 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr depthVsp->getHeight() != deviceRes->depth_resolution->height) { continue; } - } else if (depthVsp->getWidth() != modelConfig->default_depth_resolution.width || - depthVsp->getHeight() != modelConfig->default_depth_resolution.height) { + } else if (depthVsp->getWidth() != modelConfig.default_depth_resolution.width || + depthVsp->getHeight() != modelConfig.default_depth_resolution.height) { continue; } @@ -629,7 +634,7 @@ void startDevice(std::string serialNumber, const OrbbecModelConfig* modelConfig) << (format_opt.has_value() ? format_opt->to_string() : "not specifed"); if (resolution_opt.has_value() || format_opt.has_value()) { // Create the pipeline - auto config = createHwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt, modelConfig); + auto config = createHwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt, *modelConfig); if (config == nullptr) { VIAM_SDK_LOG(warn) << "Device " << serialNumber << " does not support hardware depth-to-color alignment, trying software alignment"; @@ -811,7 +816,7 @@ void stopDevice(std::string serialNumber, std::string resourceName) { // HELPERS END // RESOURCE BEGIN -void Orbbec::validate_sensor(std::pair const& sensor_pair) { +void Orbbec::validate_sensor(std::pair const& sensor_pair, const OrbbecModelConfig& modelConfig) { auto const& sensor_type = sensor_pair.first; auto const& sensor = sensor_pair.second.get(); if (!sensor) { @@ -842,10 +847,10 @@ void Orbbec::validate_sensor(std::pair const throw std::invalid_argument("sensor format must be a string"); } if (sensor_type == "color") { - if (!supported_color_formats.count(*format)) { + if (!modelConfig.supported_color_formats.count(*format)) { std::ostringstream buffer; buffer << "color sensor format must be one of: "; - for (const auto& type : supported_color_formats) { + for (const auto& type : modelConfig.supported_color_formats) { buffer << type << " "; } VIAM_SDK_LOG(error) << buffer.str(); @@ -853,10 +858,10 @@ void Orbbec::validate_sensor(std::pair const } } else if (sensor_type == "depth") { - if (!supported_depth_formats.count(*format)) { + if (!modelConfig.supported_depth_formats.count(*format)) { std::ostringstream buffer; buffer << "depth sensor format must be one of: "; - for (const auto& type : supported_depth_formats) { + for (const auto& type : modelConfig.supported_depth_formats) { buffer << type << " "; } VIAM_SDK_LOG(error) << buffer.str(); @@ -891,7 +896,7 @@ std::vector Orbbec::validateAstra2(vsdk::ResourceConfig cfg) { } if (sensors) { for (auto const& sensor_pair : *sensors) { - validate_sensor(sensor_pair); + validate_sensor(sensor_pair, ASTRA2_CONFIG); } } auto color_width_uint32 = @@ -951,7 +956,7 @@ std::vector Orbbec::validateGemini335Le(vsdk::ResourceConfig cfg) { } if (sensors) { for (auto const& sensor_pair : *sensors) { - validate_sensor(sensor_pair); + validate_sensor(sensor_pair, GEMINI_335LE_CONFIG); } } auto color_width_uint32 = @@ -1180,7 +1185,7 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro res_format_opt = config_by_serial().at(serial_number).device_format; } - validateColorFrame(color, res_format_opt); + validateColorFrame(color, res_format_opt, *model_config_); vsdk::Camera::raw_image response = encodeColorFrame(color); VIAM_RESOURCE_LOG(debug) << "[get_image] end"; return response; @@ -1332,7 +1337,7 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte if (should_process_color) { color = fs->getFrame(OB_FRAME_COLOR); - validateColorFrame(color, device_format_opt); + validateColorFrame(color, device_format_opt, *model_config_); vsdk::Camera::raw_image color_image = encodeColorFrame(color); response.images.emplace_back(std::move(color_image)); @@ -1340,7 +1345,7 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte if (should_process_depth) { depth = fs->getFrame(OB_FRAME_DEPTH); - validateDepthFrame(depth, device_format_opt); + validateDepthFrame(depth, device_format_opt, *model_config_); unsigned char* depthData = (unsigned char*)depth->getData(); if (depthData == nullptr) { @@ -1409,6 +1414,11 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { for (auto const& [key, value] : command) { if (key == firmware_key) { vsdk::ProtoStruct resp = viam::sdk::ProtoStruct{}; + if(model_config_->model_name == "Gemini 335Le") { + VIAM_RESOURCE_LOG(error) << "[do_command] firmware update not supported for this model"; + resp.emplace(firmware_key, std::string("firmware update not supported for this model")); + return resp; + } if (firmware_version_.find(model_config_->min_firmware_version) != std::string::npos) { std::ostringstream buffer; buffer << "device firmware already on version " << model_config_->min_firmware_version; @@ -1541,8 +1551,8 @@ vsdk::Camera::point_cloud Orbbec::get_point_cloud(std::string mime_type, const v std::shared_ptr color = fs->getFrame(OB_FRAME_COLOR); std::shared_ptr depth = fs->getFrame(OB_FRAME_DEPTH); - validateColorFrame(color, std::nullopt); - validateDepthFrame(depth, std::nullopt); + validateColorFrame(color, std::nullopt, *model_config_); + validateDepthFrame(depth, std::nullopt, *model_config_); std::uint8_t* colorData = (std::uint8_t*)color->getData(); if (colorData == nullptr) { @@ -1669,7 +1679,7 @@ void registerDevice(std::string serialNumber, std::shared_ptr dev) { std::shared_ptr deviceInfo = dev->getDeviceInfo(); const OrbbecModelConfig* modelConfig = &OrbbecModelConfig::forDevice(deviceInfo->name()); - std::shared_ptr config = createHwD2CAlignConfig(pipe, std::nullopt, std::nullopt, modelConfig); + std::shared_ptr config = createHwD2CAlignConfig(pipe, std::nullopt, std::nullopt, *modelConfig); if (config == nullptr) { VIAM_SDK_LOG(warn) << "Device " << serialNumber << " does not support hardware depth-to-color alignment, trying software alignment"; diff --git a/src/module/orbbec.hpp b/src/module/orbbec.hpp index b0a9e1e3..efc2f1fb 100644 --- a/src/module/orbbec.hpp +++ b/src/module/orbbec.hpp @@ -103,8 +103,11 @@ struct OrbbecModelConfig { Resolution default_depth_resolution; std::optional firmware_url; std::string min_firmware_version; - bool is_ethernet; std::map>, std::greater> color_to_depth_supported_resolutions; + std::unordered_set supported_color_formats; + std::unordered_set supported_depth_formats; + std::string default_color_format; + std::string default_depth_format; // Get config for a device name static const OrbbecModelConfig& forDevice(const std::string& device_name); @@ -148,10 +151,6 @@ class Orbbec final : public viam::sdk::Camera, public viam::sdk::Reconfigurable static viam::sdk::GeometryConfig geometry; static viam::sdk::Model model_astra2; static viam::sdk::Model model_gemini_335le; - static const std::unordered_set supported_color_formats; - static const std::unordered_set supported_depth_formats; - static const std::string default_color_format; - static const std::string default_depth_format; private: std::shared_ptr ob_ctx_; @@ -160,7 +159,13 @@ class Orbbec final : public viam::sdk::Camera, public viam::sdk::Reconfigurable std::string serial_number_; const OrbbecModelConfig* model_config_; static std::unique_ptr configure(viam::sdk::Dependencies deps, viam::sdk::ResourceConfig cfg); - static void validate_sensor(std::pair const& sensor_pair); + static void validate_sensor(std::pair const& sensor_pair, const OrbbecModelConfig& modelConfig); + + // Create a config for hardware depth-to-color alignment + static std::shared_ptr createHwD2CAlignConfig(std::shared_ptr pipe, + std::optional deviceRes, + std::optional deviceFormat, + const OrbbecModelConfig& modelConfig); }; } // namespace orbbec From da15895c6f8c424776ab8a339c433f1e76bb6b6a Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 13:10:07 -0400 Subject: [PATCH 08/33] Returning timeSinceFrameUs logic to previous logic of returning 0 when nowUs <= imageTimeUs Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/module/orbbec.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 23bafb87..209a9035 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -195,7 +195,7 @@ uint64_t timeSinceFrameUs(uint64_t nowUs, uint64_t imageTimeUs) { if (nowUs > imageTimeUs) { return nowUs - imageTimeUs; } - return imageTimeUs - nowUs; + return 0; } namespace { From f01134a2d445456d684e58ce7cd65033f9e0c378 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 13:10:57 -0400 Subject: [PATCH 09/33] Reenabling checkFirmwareVersion in get_image() Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/module/orbbec.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 209a9035..04f88d80 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -1163,7 +1163,7 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro serial_number = serial_number_; } - // checkFirmwareVersion(firmware_version_); + checkFirmwareVersion(firmware_version_); std::shared_ptr fs = nullptr; { From 9042e6b5b3d72eaf08e57af5376ad77a7070bee2 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 13:21:14 -0400 Subject: [PATCH 10/33] In Update Firmware making sure the zip name is at least 4 characters Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/module/orbbec_firmware.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/module/orbbec_firmware.cpp b/src/module/orbbec_firmware.cpp index 7758ee8d..82ad26cc 100644 --- a/src/module/orbbec_firmware.cpp +++ b/src/module/orbbec_firmware.cpp @@ -79,7 +79,8 @@ void updateFirmware(std::unique_ptr& my_dev, for (zip_int64_t i = 0; i < numEntries; i++) { const char* name = zip_get_name(zip, i, 0); - if (name && std::string(name).substr(std::string(name).length() - 4) == ".bin") { + if (name && std::string(name).length() >= 4 && + std::string(name).substr(std::string(name).length() - 4) == ".bin") { fileName = name; break; } From 362f7a4189aff80cc2d302010c4339727bc0a5ca Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 13:21:59 -0400 Subject: [PATCH 11/33] Actually performing the firmwareUpdate Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/module/orbbec_firmware.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/module/orbbec_firmware.cpp b/src/module/orbbec_firmware.cpp index 82ad26cc..905ec324 100644 --- a/src/module/orbbec_firmware.cpp +++ b/src/module/orbbec_firmware.cpp @@ -120,8 +120,9 @@ void updateFirmware(std::unique_ptr& my_dev, // Update firmware on device if (my_dev && my_dev->device) { try { - // Note: The actual firmware update would depend on the Orbbec SDK API - // This is a placeholder for the actual implementation + // Perform the actual firmware update using the Orbbec SDK API + // Example: updateFirmware method (replace with actual API if different) + my_dev->device->updateFirmware(binData.data(), static_cast(binData.size())); std::cout << "Firmware update completed successfully" << std::endl; } catch (const std::exception& e) { throw std::runtime_error("Firmware update failed: " + std::string(e.what())); From c2f7cf5f3909d1de9d5e238a6131127e4598ac9c Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 14:10:54 -0400 Subject: [PATCH 12/33] [[RSDK-12362] Returning firmwareUpdate logic to its original state, but just moved to its own file --- src/module/orbbec.cpp | 4 +- src/module/orbbec_firmware.cpp | 213 ++++++++++++++++++++------------- src/module/orbbec_firmware.hpp | 5 +- 3 files changed, 134 insertions(+), 88 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 04f88d80..e141c91d 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -1163,7 +1163,7 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro serial_number = serial_number_; } - checkFirmwareVersion(firmware_version_); + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); std::shared_ptr fs = nullptr; { @@ -1433,7 +1433,7 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { VIAM_RESOURCE_LOG(info) << "Updating device firmware..."; try { if (model_config_->firmware_url.has_value()) { - firmware::updateFirmware(dev, ob_ctx_, model_config_->firmware_url.value()); + firmware::updateFirmware(dev, ob_ctx_, model_config_->firmware_url.value(), logger_); } else { throw std::runtime_error("Firmware update not supported for this model"); } diff --git a/src/module/orbbec_firmware.cpp b/src/module/orbbec_firmware.cpp index 905ec324..96f9f7ef 100644 --- a/src/module/orbbec_firmware.cpp +++ b/src/module/orbbec_firmware.cpp @@ -25,112 +25,155 @@ namespace orbbec { namespace firmware { -// Callback for writing downloaded data to buffer -size_t writeFileCallback(void* contents, size_t size, size_t nmemb, void* userp) { - size_t realsize = size * nmemb; - std::vector* buffer = static_cast*>(userp); - buffer->insert(buffer->end(), static_cast(contents), static_cast(contents) + realsize); - return realsize; -} - -void updateFirmware(std::unique_ptr& my_dev, - std::shared_ptr ctx, - const std::string& firmware_url) { - std::cout << "Starting firmware update from: " << firmware_url << std::endl; + size_t writeFileCallback(void* contents, size_t size, size_t nmemb, void* userp) { + auto* buffer = static_cast*>(userp); + size_t totalSize = size * nmemb; + buffer->insert(buffer->end(), static_cast(contents), static_cast(contents) + totalSize); + return totalSize; + } - // Download firmware file - std::vector zipBuffer; + template + struct Cleanup { + using pointer_type = std::tuple_element_t<0, boost::callable_traits::args_t>; + using value_type = std::remove_pointer_t; - CURL* curl = curl_easy_init(); - if (!curl) { - throw std::runtime_error("Failed to initialize CURL"); - } + void operator()(pointer_type p) { + if (p != nullptr) { + cleanup_fp(p); + } + } + }; - curl_easy_setopt(curl, CURLOPT_URL, firmware_url.c_str()); - curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L); - curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, writeFileCallback); - curl_easy_setopt(curl, CURLOPT_WRITEDATA, &zipBuffer); + template + using CleanupPtr = std::unique_ptr::value_type, Cleanup>; - CURLcode res = curl_easy_perform(curl); - curl_easy_cleanup(curl); + void updateFirmware(std::unique_ptr& my_dev, std::shared_ptr ctx, const std::string& firmwareUrl, viam::sdk::LogSource &logger) { + // On linux, orbbec reccomends to set libuvc backend for firmware update + #if defined(__linux__) + ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC); + #endif - if (res != CURLE_OK) { - throw std::runtime_error("Failed to download firmware: " + std::string(curl_easy_strerror(res))); - } + CleanupPtr curl(curl_easy_init()); + if (!curl) { + throw std::invalid_argument("curl easy init failed"); + } - std::cout << "Downloaded " << zipBuffer.size() << " bytes" << std::endl; + // Download the firmware and write it to a buffer + std::vector zipBuffer; + curl_easy_setopt(curl.get(), CURLOPT_URL, firmwareUrl.c_str()); + curl_easy_setopt(curl.get(), CURLOPT_FOLLOWLOCATION, 1L); + curl_easy_setopt(curl.get(), CURLOPT_WRITEFUNCTION, writeFileCallback); + curl_easy_setopt(curl.get(), CURLOPT_WRITEDATA, &zipBuffer); + CURLcode res = curl_easy_perform(curl.get()); + if (res != CURLE_OK) { + std::ostringstream buffer; + buffer << "curl early perform failed: " << curl_easy_strerror(res); + throw std::invalid_argument(buffer.str()); + } - // Extract ZIP file - zip_error_t zipError; - zip_source_t* src = zip_source_buffer_create(zipBuffer.data(), zipBuffer.size(), 0, &zipError); - if (!src) { - throw std::runtime_error("Failed to create zip source"); - } + std::vector binData; + zip_error_t ziperror; + zip_error_init(&ziperror); - zip_t* zip = zip_open_from_source(src, 0, &zipError); - if (!zip) { - zip_source_free(src); - throw std::runtime_error("Failed to open zip file"); - } + zip_source_t* src = zip_source_buffer_create(zipBuffer.data(), zipBuffer.size(), 0, &ziperror); + if (!src) { + std::ostringstream buffer; + buffer << "failed to create zip buffer: " << zip_error_strerror(&ziperror); + throw std::runtime_error(buffer.str()); + } - // Find firmware file (assuming it's a .bin file) - zip_int64_t numEntries = zip_get_num_entries(zip, 0); - std::string fileName; + // Ensure src cleanup if zip_open fails + CleanupPtr srcCleanup(src); - for (zip_int64_t i = 0; i < numEntries; i++) { - const char* name = zip_get_name(zip, i, 0); - if (name && std::string(name).length() >= 4 && - std::string(name).substr(std::string(name).length() - 4) == ".bin") { - fileName = name; - break; + // If this succeeds, zip takes ownership of src, so src will be freed when zip_close is called. + zip_t* zip = zip_open_from_source(src, 0, &ziperror); + if (!zip) { + std::ostringstream buffer; + buffer << "failed to open zip from source: " << zip_error_strerror(&ziperror); + throw std::runtime_error(buffer.str()); } - } - if (fileName.empty()) { - zip_close(zip); - throw std::runtime_error("No firmware file found in zip"); - } + srcCleanup.release(); + CleanupPtr zipCleanup(zip); - std::cout << "Found firmware file: " << fileName << std::endl; + if (zip_get_num_entries(zip, 0) != 1) { + throw std::runtime_error("unexpected number of files in firmware zip"); + } - // Extract firmware file - zip_stat_t stats; - if (zip_stat(zip, fileName.c_str(), 0, &stats) != 0) { - zip_close(zip); - throw std::runtime_error("Failed to get file stats"); - } + const char* fileName = zip_get_name(zip, 0, 0); + if (!fileName) { + throw std::runtime_error("couldn't get bin file name"); + } - zip_file_t* binFile = zip_fopen(zip, fileName.c_str(), 0); - if (!binFile) { - zip_close(zip); - throw std::runtime_error("Failed to open firmware file"); - } + CleanupPtr binFile(zip_fopen(zip, fileName, 0)); + if (!binFile) { + throw std::runtime_error("failed to open the firmware bin file"); + } - std::vector binData(stats.size); - zip_int64_t bytesRead = zip_fread(binFile, binData.data(), stats.size); - zip_fclose(binFile); - zip_close(zip); + zip_stat_t stats; + zip_stat_init(&stats); + if (zip_stat(zip, fileName, 0, &stats) != 0) { + throw std::invalid_argument("failed to stat file"); + } - if (bytesRead != static_cast(stats.size)) { - throw std::runtime_error("Failed to read complete firmware file"); - } + binData.resize(stats.size); + zip_int64_t bytesRead = zip_fread(binFile.get(), binData.data(), stats.size); + if (bytesRead == -1) { + zip_error_t* err = zip_file_get_error(binFile.get()); + std::ostringstream buffer; + buffer << "failed to read bin: " << zip_error_strerror(err); + throw std::runtime_error(buffer.str()); + } - std::cout << "Extracted " << bytesRead << " bytes of firmware data" << std::endl; + if (bytesRead != stats.size) { + std::ostringstream buffer; + buffer << "failed to fully read binary file, file size: " << stats.size << "bytes read: " << bytesRead; + throw std::runtime_error(buffer.str()); + } - // Update firmware on device - if (my_dev && my_dev->device) { + auto firmwareUpdateCallback = [](OBFwUpdateState state, const char* message, uint8_t percent) { + switch (state) { + case STAT_VERIFY_SUCCESS: + std::cout << "Image file verification success\n"; + case STAT_FILE_TRANSFER: + std::cout << "File transfer in progress\n"; + break; + case STAT_DONE: + std::cout << "Update completed\n"; + break; + case STAT_IN_PROGRESS: + std::cout << "Upgrade in progress\n"; + break; + case STAT_START: + std::cout << "Starting the upgrade\n"; + break; + case STAT_VERIFY_IMAGE: + std::cout << "Verifying image file\n"; + break; + default: + std::cerr << "Unknown status or error\n"; + break; + } + std::cout << "Firmware update in progress: " << message << " upgrade " << static_cast(percent) << "% complete\n"; + }; + + bool executeAsync = false; try { - // Perform the actual firmware update using the Orbbec SDK API - // Example: updateFirmware method (replace with actual API if different) - my_dev->device->updateFirmware(binData.data(), static_cast(binData.size())); - std::cout << "Firmware update completed successfully" << std::endl; - } catch (const std::exception& e) { - throw std::runtime_error("Firmware update failed: " + std::string(e.what())); + my_dev->device->updateFirmwareFromData(binData.data(), binData.size(), std::move(firmwareUpdateCallback), executeAsync); + VIAM_SDK_LOG_IMPL(logger, info) << "firmware update successful!"; + } catch (...) { + VIAM_SDK_LOG_IMPL(logger, error) << "firmware update failed!"; + // Reset UVC backend type before re-throwing + #if defined(__linux__) + ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_AUTO); + #endif + throw; } - } else { - throw std::runtime_error("No device available for firmware update"); + + // Reset UVC backend type on success + #if defined(__linux__) + ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_AUTO); + #endif } -} - } // namespace firmware } // namespace orbbec \ No newline at end of file diff --git a/src/module/orbbec_firmware.hpp b/src/module/orbbec_firmware.hpp index ce9e4777..86686427 100644 --- a/src/module/orbbec_firmware.hpp +++ b/src/module/orbbec_firmware.hpp @@ -20,6 +20,8 @@ #include +#include + namespace orbbec { struct ViamOBDevice; @@ -32,7 +34,8 @@ size_t writeFileCallback(void* contents, size_t size, size_t nmemb, void* userp) // Main firmware update function void updateFirmware(std::unique_ptr& my_dev, std::shared_ptr ctx, - const std::string& firmware_url); + const std::string& firmware_url, + viam::sdk::LogSource &logger); } // namespace firmware } // namespace orbbec \ No newline at end of file From 5e0be35ef1ef2664398b92bb2f42dbe1c4b9cbb4 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 14:19:51 -0400 Subject: [PATCH 13/33] [[RSDK-12362] Improving firmware_update logging and responses --- src/module/orbbec.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index e141c91d..c404fc68 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -1413,6 +1413,7 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { std::unique_ptr& dev = search->second; for (auto const& [key, value] : command) { if (key == firmware_key) { + VIAM_RESOURCE_LOG(info) << "Received firmware update command"; vsdk::ProtoStruct resp = viam::sdk::ProtoStruct{}; if(model_config_->model_name == "Gemini 335Le") { VIAM_RESOURCE_LOG(error) << "[do_command] firmware update not supported for this model"; @@ -1421,18 +1422,20 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { } if (firmware_version_.find(model_config_->min_firmware_version) != std::string::npos) { std::ostringstream buffer; - buffer << "device firmware already on version " << model_config_->min_firmware_version; + buffer << "Device firmware already on version " << model_config_->min_firmware_version; resp.emplace(firmware_key, buffer.str()); - break; + VIAM_RESOURCE_LOG(info) << buffer.str(); + return resp; } + VIAM_RESOURCE_LOG(info) << "Updating device firmware..."; if (dev->started) { dev->pipe->stop(); dev->started = false; } - VIAM_RESOURCE_LOG(info) << "Updating device firmware..."; try { if (model_config_->firmware_url.has_value()) { + VIAM_RESOURCE_LOG(info) << "Updating device firmware from URL: " << model_config_->firmware_url.value(); firmware::updateFirmware(dev, ob_ctx_, model_config_->firmware_url.value(), logger_); } else { throw std::runtime_error("Firmware update not supported for this model"); From 0b3c31ec5b8bf15abeebfe178d67b9a93e5c78c0 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 14:27:52 -0400 Subject: [PATCH 14/33] [RSDK-12362] Enabled and positively tested firmware update on Gemini335LE camera --- src/module/orbbec.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index c404fc68..820b640c 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -90,8 +90,8 @@ namespace { .viam_model_suffix = "gemini_335le", .default_color_resolution = {1280, 800}, .default_depth_resolution = {1280, 800}, - .firmware_url = "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Gemini330_Release_1.6.00.zip", - .min_firmware_version = "1.5.31", + .firmware_url = "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Gemini330_Release_1.5.55.zip", + .min_firmware_version = "1.5.55", .color_to_depth_supported_resolutions = {{{1280, 800}, {{1280, 800}, {848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio {{848, 530}, {{848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio {{640, 400}, {{640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio @@ -1415,11 +1415,6 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { if (key == firmware_key) { VIAM_RESOURCE_LOG(info) << "Received firmware update command"; vsdk::ProtoStruct resp = viam::sdk::ProtoStruct{}; - if(model_config_->model_name == "Gemini 335Le") { - VIAM_RESOURCE_LOG(error) << "[do_command] firmware update not supported for this model"; - resp.emplace(firmware_key, std::string("firmware update not supported for this model")); - return resp; - } if (firmware_version_.find(model_config_->min_firmware_version) != std::string::npos) { std::ostringstream buffer; buffer << "Device firmware already on version " << model_config_->min_firmware_version; From 260be4755bd7245741e051905bdf5a909064371c Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 14:37:11 -0400 Subject: [PATCH 15/33] [RSDK-12362] Updating README.md --- README.md | 65 ++++++++++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 60 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index c190f716..6b5034e0 100644 --- a/README.md +++ b/README.md @@ -35,7 +35,7 @@ The following attribute template can be used to configure this model: ``` #### Configuration Attributes -The following attributes are available for this model: +The following attributes are available for the Astra2 model: | Name | Type | Inclusion | Description | |---------------|--------|-----------|----------------------------| @@ -56,7 +56,7 @@ The following attributes are available for this model: | `depth` | `Y16` | -#### `height`/`width` available combinations +#### `width`/`height` available combinations | Color | Depth | |-------|-------| | `1920x1080` | `1600x1200`, `800X600`, `400X300` | @@ -66,8 +66,63 @@ The following attributes are available for this model: | `640X480` | `800X600`, `400X300` | | `640X360` | `800X600`, `400X300` | +## Model viam:orbbec:gemini_335le -### Attributes +Use [Orbbec cameras](https://www.orbbec.com/gemini-335le/). + +### Configuration +The following attribute template can be used to configure this model: + +```json +{ + "serial_number": "AARY14100EF", + "sensors": { + "depth": { + "height": 1200, + "width": 1600, + "format": "Y16" + }, + "color": { + "width": 1920, + "height": 1080, + "format": "MJPG" + } + } +} +``` +#### Configuration Attributes + +The following attributes are available for the Astra2 model: + +| Name | Type | Inclusion | Description | +|---------------|--------|-----------|----------------------------| +| `serial_number` | string | **Required** | The serial number of the specific Orbbec camera to use. This number is printed on the device. The serial number of each plugged-in and available orbbec camera will be logged on module startup. | +|`sensors` | struct | **Optional** | The configuration of the color and depth sensors | + +#### `sensor` attributes: +| Name | Type | Inclusion | Description | +|------|------|-----------|-------------| +| `height` | string | **Optional** | Native camera sensor height in pixels | +| `width` | string | **Optional** | Native camera sensor width in pixels | +| `format` | string | **Optional** | Native camera format | + +#### `sensor` formats +| Sensor | Formats | +|--------|---------| +| `color` | `MJPG`| +| `depth` | `Y16` | + + +#### `width`/`height` available combinations +| Color | Depth | +|-------|-------| +| `1280X800` | `1280X800`, `848X530`, `640X400`, `424X266`, `320X200` | +| `848X530` | `640X400`, `424X266`, `320X200` | +| `640X400` | `640X400`, `424X266`, `320X200` | +| `640X480` | `640X480` | + + +## Attributes A call to get_attributes will return the camera attributes in [this struct](https://github.com/viamrobotics/viam-cpp-sdk/blob/43deea420f572e6b61b6fbd519e09b2520f05676/src/viam/sdk/components/camera.hpp#L58) Bear in mind that the distortion parameters contained in that struct are not named, i.e. they are contained in a vector of doubles. So they must be parsed following the order in which they are being stored, which is as follows: @@ -83,10 +138,10 @@ Bear in mind that the distortion parameters contained in that struct are not nam | 6 | k5 | | 7 | k6 | -### DoCommand +## DoCommand You can use DoCommand to upgrade the firmware of your device to the required version. -Update the firmware to v2.8.20. If running on macOS, you must unplug and replug the device after this returns. +Update the astra2 firmware to v2.8.20 and gemini_335le to v1.5.55. If running on macOS, you must manually unplug and replug the device after this returns. **WARNING**: Do not unplug the device while the firmware update is in progress. { From a745de5e5e83c8c1a408fb967690355c610157e8 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 14:42:09 -0400 Subject: [PATCH 16/33] Update src/module/orbbec_firmware.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/module/orbbec_firmware.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/module/orbbec_firmware.cpp b/src/module/orbbec_firmware.cpp index 96f9f7ef..a2f06f59 100644 --- a/src/module/orbbec_firmware.cpp +++ b/src/module/orbbec_firmware.cpp @@ -48,7 +48,7 @@ namespace firmware { using CleanupPtr = std::unique_ptr::value_type, Cleanup>; void updateFirmware(std::unique_ptr& my_dev, std::shared_ptr ctx, const std::string& firmwareUrl, viam::sdk::LogSource &logger) { - // On linux, orbbec reccomends to set libuvc backend for firmware update + // On linux, orbbec recommends to set libuvc backend for firmware update #if defined(__linux__) ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC); #endif From a1ed50993212727e66d6de09ff8eaf314e485edc Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 14:42:18 -0400 Subject: [PATCH 17/33] Update src/module/orbbec_firmware.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/module/orbbec_firmware.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/module/orbbec_firmware.cpp b/src/module/orbbec_firmware.cpp index a2f06f59..114c7f45 100644 --- a/src/module/orbbec_firmware.cpp +++ b/src/module/orbbec_firmware.cpp @@ -67,7 +67,7 @@ namespace firmware { CURLcode res = curl_easy_perform(curl.get()); if (res != CURLE_OK) { std::ostringstream buffer; - buffer << "curl early perform failed: " << curl_easy_strerror(res); + buffer << "curl easy perform failed: " << curl_easy_strerror(res); throw std::invalid_argument(buffer.str()); } From 0caa622d4bd329d331b1cccd284b22d12382d471 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 14:42:27 -0400 Subject: [PATCH 18/33] Update src/module/orbbec_firmware.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/module/orbbec_firmware.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/module/orbbec_firmware.cpp b/src/module/orbbec_firmware.cpp index 114c7f45..b7c89de6 100644 --- a/src/module/orbbec_firmware.cpp +++ b/src/module/orbbec_firmware.cpp @@ -135,6 +135,7 @@ namespace firmware { switch (state) { case STAT_VERIFY_SUCCESS: std::cout << "Image file verification success\n"; + break; case STAT_FILE_TRANSFER: std::cout << "File transfer in progress\n"; break; From 162a1934e11ab12658f54a9c0e824d71e1a77ab0 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 14:42:32 -0400 Subject: [PATCH 19/33] Update src/module/orbbec.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/module/orbbec.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 820b640c..b0cd6644 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -1827,7 +1827,7 @@ void startOrbbecSDK(ob::Context& ctx) { deviceInfoString << "Device " << (i + 1) << " - Name: " << deviceName << ", Serial: " << serialNumber << ", Connection: " << connectionType; - if(!ipAddress.empty()) { + if (!ipAddress.empty()) { deviceInfoString << ", IP: " << ipAddress; } VIAM_SDK_LOG(info) << deviceInfoString.str(); From 767ce349b99a092174e8158058bce21c9bdd662f Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 14:42:46 -0400 Subject: [PATCH 20/33] Update src/module/discovery.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/module/discovery.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/module/discovery.cpp b/src/module/discovery.cpp index 7202a87c..2833d6f2 100644 --- a/src/module/discovery.cpp +++ b/src/module/discovery.cpp @@ -45,7 +45,7 @@ std::vector OrbbecDiscovery::discover_resources(const vsdk deviceInfoString << "Device " << (i + 1) << " - Name: " << deviceName << ", Serial: " << serialNumber << ", Connection: " << connectionType; - if(!ipAddress.empty()) { + if (!ipAddress.empty()) { deviceInfoString << ", IP: " << ipAddress; } From e8daae1a515c3f7087752a42209c04102b63b447 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 15:05:53 -0400 Subject: [PATCH 21/33] [RSDK-12362] Fixing Windows Build --- src/module/orbbec.cpp | 60 +++++++++++++------------- src/module/orbbec_windows_registry.cpp | 2 + 2 files changed, 32 insertions(+), 30 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index b0cd6644..1982754c 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -67,39 +67,39 @@ const uint64_t maxFrameAgeUs = 1e6; // time until a frame is considered stale, // Model configurations namespace { const OrbbecModelConfig ASTRA2_CONFIG{ - .model_name = "Astra 2", - .viam_model_suffix = "astra2", - .default_color_resolution = {1280, 720}, - .default_depth_resolution = {1600, 1200}, - .firmware_url = "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Astra2_Release_2.8.20.zip", - .min_firmware_version = "2.8.20", - .color_to_depth_supported_resolutions = {{{1920, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, - {{1440, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, - {{1280, 720}, {{1600, 1200}, {800, 600}, {400, 300}}}, - {{800, 600}, {{800, 600}, {400, 300}}}, - {{640, 480}, {{800, 600}, {400, 300}}}, - {{640, 360}, {{800, 600}, {400, 300}}}}, - .supported_color_formats = {"RGB", "MJPG"}, - .supported_depth_formats = {"Y16"}, - .default_color_format = "MJPG", - .default_depth_format = "Y16" + "Astra 2", // model_name + "astra2", // viam_model_suffix + {1280, 720}, // default_color_resolution + {1600, 1200}, // default_depth_resolution + "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Astra2_Release_2.8.20.zip", // firmware_url + "2.8.20", // min_firmware_version + {{{1920, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, // Supported resolutions, 16:9 aspect ratio + {{1440, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, // Supported resolutions, 16:9 aspect ratio + {{1280, 720}, {{1600, 1200}, {800, 600}, {400, 300}}}, // Supported resolutions, 16:9 aspect ratio + {{800, 600}, {{800, 600}, {400, 300}}}, // Supported resolutions, 4:3 aspect ratio + {{640, 480}, {{800, 600}, {400, 300}}}, // Supported resolutions, 4:3 aspect ratio + {{640, 360}, {{800, 600}, {400, 300}}}}, // Supported resolutions, 4:3 aspect ratio + {"RGB", "MJPG"}, // supported_color_formats + {"Y16"}, // supported_depth_formats + "MJPG", // default_color_format + "Y16" // default_depth_format }; const OrbbecModelConfig GEMINI_335LE_CONFIG{ - .model_name = "Gemini 335Le", - .viam_model_suffix = "gemini_335le", - .default_color_resolution = {1280, 800}, - .default_depth_resolution = {1280, 800}, - .firmware_url = "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Gemini330_Release_1.5.55.zip", - .min_firmware_version = "1.5.55", - .color_to_depth_supported_resolutions = {{{1280, 800}, {{1280, 800}, {848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio - {{848, 530}, {{848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio - {{640, 400}, {{640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio - {{640, 480}, {{640, 480}}}}, // 4:3 aspect ratio - .supported_color_formats = {"MJPG"}, - .supported_depth_formats = {"Y16"}, - .default_color_format = "MJPG", - .default_depth_format = "Y16" + "Gemini 335Le", // model_name + "gemini_335le", // viam_model_suffix + {1280, 800}, // default_color_resolution + {1280, 800}, // default_depth_resolution + "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Gemini330_Release_1.5.55.zip", // firmware_url + "1.5.55", // min_firmware_version + {{{1280, 800}, {{1280, 800}, {848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // Supported resolutions, 16:10 aspect ratio + {{848, 530}, {{848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio + {{640, 400}, {{640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio + {{640, 480}, {{640, 480}}}}, // 4:3 aspect ratio + {"MJPG"}, // supported_color_formats + {"Y16"}, // supported_depth_formats + "MJPG", // default_color_format + "Y16" // default_depth_format }; } diff --git a/src/module/orbbec_windows_registry.cpp b/src/module/orbbec_windows_registry.cpp index 7347e9ab..e52340aa 100644 --- a/src/module/orbbec_windows_registry.cpp +++ b/src/module/orbbec_windows_registry.cpp @@ -18,10 +18,12 @@ #include #include #include +#include #include #include #include +#include namespace orbbec { namespace windows_registry { From 4915a348631de6e716db88af948c785f9f9e9eae Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 15:39:06 -0400 Subject: [PATCH 22/33] [RSDK-12362] Creating function distortionTypeToString --- src/module/device_control.hpp | 27 +++++++++++++++++++++++++++ src/module/orbbec.cpp | 24 +----------------------- 2 files changed, 28 insertions(+), 23 deletions(-) diff --git a/src/module/device_control.hpp b/src/module/device_control.hpp index 6ff05cd7..5e88e120 100644 --- a/src/module/device_control.hpp +++ b/src/module/device_control.hpp @@ -84,6 +84,32 @@ inline std::string propertyTypeToString(OBPropertyType type) { } } +inline std::string distortionTypeToString(OBCameraDistortionModel model) { + switch (model) { + case OB_DISTORTION_NONE: + return "NONE"; + break; + case OB_DISTORTION_MODIFIED_BROWN_CONRADY: + return "MODIFIED_BROWN_CONRADY"; + break; + case OB_DISTORTION_INVERSE_BROWN_CONRADY: + return "INVERSE_BROWN_CONRADY"; + break; + case OB_DISTORTION_BROWN_CONRADY: + return "BROWN_CONRADY"; + break; + case OB_DISTORTION_BROWN_CONRADY_K6: + return "BROWN_CONRADY_K6"; + break; + case OB_DISTORTION_KANNALA_BRANDT4: + return "KANNALA_BRANDT4"; + break; + default: + return "UNKNOWN"; + break; + } +} + viam::sdk::ProtoStruct getOrbbecSDKVersion(std::string const& command) { viam::sdk::ProtoStruct resp; std::stringstream ss; @@ -539,6 +565,7 @@ viam::sdk::ProtoStruct getCameraParams(std::shared_ptr pipe) { distortion_struct["k6"] = static_cast(distortion.k6); distortion_struct["p1"] = static_cast(distortion.p1); distortion_struct["p2"] = static_cast(distortion.p2); + distortion_struct["model"] = distortionTypeToString(distortion.model); profile["distortion"] = distortion_struct; result[sensorName] = profile; diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 1982754c..573dbd97 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -1238,30 +1238,8 @@ vsdk::Camera::properties Orbbec::get_properties() { p.intrinsic_parameters.focal_y_px = intrinsic_props.fy; p.intrinsic_parameters.center_x_px = intrinsic_props.cx; p.intrinsic_parameters.center_y_px = intrinsic_props.cy; + p.distortion_parameters.model = device_control::distortionTypeToString(distortion_props.model); - switch (distortion_props.model) { - case OB_DISTORTION_NONE: - p.distortion_parameters.model = "NONE"; - break; - case OB_DISTORTION_MODIFIED_BROWN_CONRADY: - p.distortion_parameters.model = "MODIFIED_BROWN_CONRADY"; - break; - case OB_DISTORTION_INVERSE_BROWN_CONRADY: - p.distortion_parameters.model = "INVERSE_BROWN_CONRADY"; - break; - case OB_DISTORTION_BROWN_CONRADY: - p.distortion_parameters.model = "BROWN_CONRADY"; - break; - case OB_DISTORTION_BROWN_CONRADY_K6: - p.distortion_parameters.model = "BROWN_CONRADY_K6"; - break; - case OB_DISTORTION_KANNALA_BRANDT4: - p.distortion_parameters.model = "KANNALA_BRANDT4"; - break; - default: - p.distortion_parameters.model = "UNKNOWN"; - break; - } // TODO: These should be named parameters in the struct, not relying on order // If this is ever changed, make sure to update the README p.distortion_parameters.parameters = std::vector{distortion_props.p1, From 30ca885a9dd7c05c50c6fe4111aae48cbb774246 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Mon, 27 Oct 2025 15:47:05 -0400 Subject: [PATCH 23/33] [RSDK-12362] Making lint happy --- src/module/device_control.hpp | 44 ++-- src/module/discovery.cpp | 27 ++- src/module/orbbec.cpp | 252 ++++++++++----------- src/module/orbbec.hpp | 10 +- src/module/orbbec_firmware.cpp | 301 +++++++++++++------------ src/module/orbbec_firmware.hpp | 12 +- src/module/orbbec_windows_registry.cpp | 8 +- src/module/orbbec_windows_registry.hpp | 4 +- 8 files changed, 330 insertions(+), 328 deletions(-) diff --git a/src/module/device_control.hpp b/src/module/device_control.hpp index 5e88e120..837bf5a1 100644 --- a/src/module/device_control.hpp +++ b/src/module/device_control.hpp @@ -86,28 +86,28 @@ inline std::string propertyTypeToString(OBPropertyType type) { inline std::string distortionTypeToString(OBCameraDistortionModel model) { switch (model) { - case OB_DISTORTION_NONE: - return "NONE"; - break; - case OB_DISTORTION_MODIFIED_BROWN_CONRADY: - return "MODIFIED_BROWN_CONRADY"; - break; - case OB_DISTORTION_INVERSE_BROWN_CONRADY: - return "INVERSE_BROWN_CONRADY"; - break; - case OB_DISTORTION_BROWN_CONRADY: - return "BROWN_CONRADY"; - break; - case OB_DISTORTION_BROWN_CONRADY_K6: - return "BROWN_CONRADY_K6"; - break; - case OB_DISTORTION_KANNALA_BRANDT4: - return "KANNALA_BRANDT4"; - break; - default: - return "UNKNOWN"; - break; - } + case OB_DISTORTION_NONE: + return "NONE"; + break; + case OB_DISTORTION_MODIFIED_BROWN_CONRADY: + return "MODIFIED_BROWN_CONRADY"; + break; + case OB_DISTORTION_INVERSE_BROWN_CONRADY: + return "INVERSE_BROWN_CONRADY"; + break; + case OB_DISTORTION_BROWN_CONRADY: + return "BROWN_CONRADY"; + break; + case OB_DISTORTION_BROWN_CONRADY_K6: + return "BROWN_CONRADY_K6"; + break; + case OB_DISTORTION_KANNALA_BRANDT4: + return "KANNALA_BRANDT4"; + break; + default: + return "UNKNOWN"; + break; + } } viam::sdk::ProtoStruct getOrbbecSDKVersion(std::string const& command) { diff --git a/src/module/discovery.cpp b/src/module/discovery.cpp index 2833d6f2..da3e0aa8 100644 --- a/src/module/discovery.cpp +++ b/src/module/discovery.cpp @@ -42,13 +42,12 @@ std::vector OrbbecDiscovery::discover_resources(const vsdk std::string ipAddress = devList->ipAddress(i); std::stringstream deviceInfoString; - deviceInfoString << "Device " << (i + 1) << " - Name: " << deviceName - << ", Serial: " << serialNumber + deviceInfoString << "Device " << (i + 1) << " - Name: " << deviceName << ", Serial: " << serialNumber << ", Connection: " << connectionType; if (!ipAddress.empty()) { deviceInfoString << ", IP: " << ipAddress; } - + VIAM_RESOURCE_LOG(info) << deviceInfoString.str(); std::ostringstream name; @@ -67,12 +66,15 @@ std::vector OrbbecDiscovery::discover_resources(const vsdk continue; } - vsdk::ResourceConfig config( - "camera", std::move(name.str()), "viam", attributes, "rdk:component:camera", - vsdk::Model("viam", "orbbec", viamModelSuffix), - vsdk::log_level::info); + vsdk::ResourceConfig config("camera", + std::move(name.str()), + "viam", + attributes, + "rdk:component:camera", + vsdk::Model("viam", "orbbec", viamModelSuffix), + vsdk::log_level::info); configs.push_back(config); - + VIAM_RESOURCE_LOG(info) << "Successfully configured device " << (i + 1) << " with serial: " << serialNumber; } catch (ob::Error& deviceError) { VIAM_RESOURCE_LOG(warn) << "Failed to get device info for device " << (i + 1) << ": " << deviceError.what(); @@ -80,11 +82,8 @@ std::vector OrbbecDiscovery::discover_resources(const vsdk } } } catch (ob::Error& e) { - VIAM_RESOURCE_LOG(error) << "Failed to discover Orbbec devices: " << e.what() - << " (function: " << e.getFunction() - << ", args: " << e.getArgs() - << ", name: " << e.getName() - << ", type: " << e.getExceptionType() << ")"; + VIAM_RESOURCE_LOG(error) << "Failed to discover Orbbec devices: " << e.what() << " (function: " << e.getFunction() + << ", args: " << e.getArgs() << ", name: " << e.getName() << ", type: " << e.getExceptionType() << ")"; VIAM_RESOURCE_LOG(warn) << "Discovery failed - check network connectivity for Ethernet cameras or USB connection for USB cameras"; } catch (const std::exception& e) { VIAM_RESOURCE_LOG(error) << "Failed to discover Orbbec devices: " << e.what(); @@ -93,7 +92,7 @@ std::vector OrbbecDiscovery::discover_resources(const vsdk VIAM_RESOURCE_LOG(error) << "Failed to discover Orbbec devices: unknown error"; VIAM_RESOURCE_LOG(warn) << "Discovery failed - check network connectivity for Ethernet cameras or USB connection for USB cameras"; } - + return configs; } diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 573dbd97..0a95e3d8 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -63,55 +63,55 @@ constexpr char service_name[] = "viam_orbbec"; const float mmToMeterMultiple = 0.001; const uint64_t maxFrameAgeUs = 1e6; // time until a frame is considered stale, in microseconds (equal to 1 sec) - // Model configurations namespace { - const OrbbecModelConfig ASTRA2_CONFIG{ - "Astra 2", // model_name - "astra2", // viam_model_suffix - {1280, 720}, // default_color_resolution - {1600, 1200}, // default_depth_resolution - "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Astra2_Release_2.8.20.zip", // firmware_url - "2.8.20", // min_firmware_version - {{{1920, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, // Supported resolutions, 16:9 aspect ratio - {{1440, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, // Supported resolutions, 16:9 aspect ratio - {{1280, 720}, {{1600, 1200}, {800, 600}, {400, 300}}}, // Supported resolutions, 16:9 aspect ratio - {{800, 600}, {{800, 600}, {400, 300}}}, // Supported resolutions, 4:3 aspect ratio - {{640, 480}, {{800, 600}, {400, 300}}}, // Supported resolutions, 4:3 aspect ratio - {{640, 360}, {{800, 600}, {400, 300}}}}, // Supported resolutions, 4:3 aspect ratio - {"RGB", "MJPG"}, // supported_color_formats - {"Y16"}, // supported_depth_formats - "MJPG", // default_color_format - "Y16" // default_depth_format - }; - - const OrbbecModelConfig GEMINI_335LE_CONFIG{ - "Gemini 335Le", // model_name - "gemini_335le", // viam_model_suffix - {1280, 800}, // default_color_resolution - {1280, 800}, // default_depth_resolution - "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Gemini330_Release_1.5.55.zip", // firmware_url - "1.5.55", // min_firmware_version - {{{1280, 800}, {{1280, 800}, {848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // Supported resolutions, 16:10 aspect ratio - {{848, 530}, {{848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio - {{640, 400}, {{640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio - {{640, 480}, {{640, 480}}}}, // 4:3 aspect ratio - {"MJPG"}, // supported_color_formats - {"Y16"}, // supported_depth_formats - "MJPG", // default_color_format - "Y16" // default_depth_format - }; -} +const OrbbecModelConfig ASTRA2_CONFIG{ + "Astra 2", // model_name + "astra2", // viam_model_suffix + {1280, 720}, // default_color_resolution + {1600, 1200}, // default_depth_resolution + "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Astra2_Release_2.8.20.zip", // firmware_url + "2.8.20", // min_firmware_version + {{{1920, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, // Supported resolutions, 16:9 aspect ratio + {{1440, 1080}, {{1600, 1200}, {800, 600}, {400, 300}}}, // Supported resolutions, 16:9 aspect ratio + {{1280, 720}, {{1600, 1200}, {800, 600}, {400, 300}}}, // Supported resolutions, 16:9 aspect ratio + {{800, 600}, {{800, 600}, {400, 300}}}, // Supported resolutions, 4:3 aspect ratio + {{640, 480}, {{800, 600}, {400, 300}}}, // Supported resolutions, 4:3 aspect ratio + {{640, 360}, {{800, 600}, {400, 300}}}}, // Supported resolutions, 4:3 aspect ratio + {"RGB", "MJPG"}, // supported_color_formats + {"Y16"}, // supported_depth_formats + "MJPG", // default_color_format + "Y16" // default_depth_format +}; + +const OrbbecModelConfig GEMINI_335LE_CONFIG{ + "Gemini 335Le", // model_name + "gemini_335le", // viam_model_suffix + {1280, 800}, // default_color_resolution + {1280, 800}, // default_depth_resolution + "https://orbbec-debian-repos-aws.s3.amazonaws.com/product/Gemini330_Release_1.5.55.zip", // firmware_url + "1.5.55", // min_firmware_version + {{{1280, 800}, {{1280, 800}, {848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // Supported resolutions, 16:10 aspect ratio + {{848, 530}, {{848, 530}, {640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio + {{640, 400}, {{640, 400}, {424, 266}, {320, 200}}}, // 16:10 aspect ratio + {{640, 480}, {{640, 480}}}}, // 4:3 aspect ratio + {"MJPG"}, // supported_color_formats + {"Y16"}, // supported_depth_formats + "MJPG", // default_color_format + "Y16" // default_depth_format +}; +} // namespace const OrbbecModelConfig& OrbbecModelConfig::forDevice(const std::string& device_name) { - if (device_name.find("Astra2") != std::string::npos || device_name.find("Astra 2") != std::string::npos) return ASTRA2_CONFIG; - if (device_name.find("Gemini 335Le") != std::string::npos) return GEMINI_335LE_CONFIG; + if (device_name.find("Astra2") != std::string::npos || device_name.find("Astra 2") != std::string::npos) + return ASTRA2_CONFIG; + if (device_name.find("Gemini 335Le") != std::string::npos) + return GEMINI_335LE_CONFIG; throw std::runtime_error("Unsupported Orbbec camera model: " + device_name); } bool OrbbecModelConfig::isSupported(const std::string& device_name) { - return (device_name.find("Astra2") != std::string::npos || - device_name.find("Astra 2") != std::string::npos || + return (device_name.find("Astra2") != std::string::npos || device_name.find("Astra 2") != std::string::npos || device_name.find("Gemini 335Le") != std::string::npos); } @@ -182,8 +182,8 @@ void checkFirmwareVersion(const std::string version, const std::string& minFirmw sscanf(minFirmwareVer.c_str(), "%d.%d.%d", &requiredMajor, &requiredMinor, &requiredPatch); if ((major < requiredMajor) || (major == requiredMajor && minor < requiredMinor) || (major == requiredMajor && minor == requiredMinor && patch < requiredPatch)) { - throw std::runtime_error("Unsupported firmware version for " + modelName + ". Required: >= " + minFirmwareVer + ", Current: " + version + - ". Call update_firmware command to upgrade."); + throw std::runtime_error("Unsupported firmware version for " + modelName + ". Required: >= " + minFirmwareVer + + ", Current: " + version + ". Call update_firmware command to upgrade."); } } @@ -201,7 +201,7 @@ uint64_t timeSinceFrameUs(uint64_t nowUs, uint64_t imageTimeUs) { namespace { // Helper function to format error messages -template +template std::string formatError(Args&&... args) { std::ostringstream buffer; (buffer << ... << args); @@ -209,13 +209,13 @@ std::string formatError(Args&&... args) { } // Validate color frame format and timestamp -void validateColorFrame(std::shared_ptr color, - const std::optional& device_format_opt, - const OrbbecModelConfig& modelConfig) { +void validateColorFrame(std::shared_ptr color, + const std::optional& device_format_opt, + const OrbbecModelConfig& modelConfig) { if (color == nullptr) { throw std::runtime_error("no color frame"); } - + // Format validation std::string frameFormat = ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()); if (modelConfig.supported_color_formats.count(frameFormat) == 0) { @@ -226,35 +226,33 @@ void validateColorFrame(std::shared_ptr color, } throw std::runtime_error(buffer.str()); } - + // Timestamp validation uint64_t nowUs = getNowUs(); uint64_t diff = timeSinceFrameUs(nowUs, color->getSystemTimeStampUs()); if (diff > maxFrameAgeUs) { throw std::runtime_error(formatError("no recent color frame: check connection, diff: ", diff, "us")); } - + // Config format validation if (device_format_opt.has_value() && device_format_opt->color_format.has_value()) { if (frameFormat != device_format_opt->color_format.value()) { - throw std::runtime_error(formatError("color format mismatch, expected ", - device_format_opt->color_format.value(), - " got ", frameFormat)); + throw std::runtime_error( + formatError("color format mismatch, expected ", device_format_opt->color_format.value(), " got ", frameFormat)); } } else if (frameFormat != modelConfig.default_color_format) { - throw std::runtime_error(formatError("color format mismatch, expected ", - modelConfig.default_color_format, " got ", frameFormat)); + throw std::runtime_error(formatError("color format mismatch, expected ", modelConfig.default_color_format, " got ", frameFormat)); } } // Validate depth frame format and timestamp void validateDepthFrame(std::shared_ptr depth, - const std::optional& device_format_opt, - const OrbbecModelConfig& modelConfig) { + const std::optional& device_format_opt, + const OrbbecModelConfig& modelConfig) { if (depth == nullptr) { throw std::runtime_error("no depth frame"); } - + // Format validation std::string frameFormat = ob::TypeHelper::convertOBFormatTypeToString(depth->getFormat()); if (modelConfig.supported_depth_formats.count(frameFormat) == 0) { @@ -265,24 +263,22 @@ void validateDepthFrame(std::shared_ptr depth, } throw std::runtime_error(buffer.str()); } - + // Timestamp validation uint64_t nowUs = getNowUs(); uint64_t diff = timeSinceFrameUs(nowUs, depth->getSystemTimeStampUs()); if (diff > maxFrameAgeUs) { throw std::runtime_error(formatError("no recent depth frame: check connection, diff: ", diff, "us")); } - + // Config format validation if (device_format_opt.has_value() && device_format_opt->depth_format.has_value()) { if (frameFormat != device_format_opt->depth_format.value()) { - throw std::runtime_error(formatError("depth format mismatch, expected ", - device_format_opt->depth_format.value(), - " got ", frameFormat)); + throw std::runtime_error( + formatError("depth format mismatch, expected ", device_format_opt->depth_format.value(), " got ", frameFormat)); } } else if (frameFormat != modelConfig.default_depth_format) { - throw std::runtime_error(formatError("depth format mismatch, expected ", - modelConfig.default_depth_format, " got ", frameFormat)); + throw std::runtime_error(formatError("depth format mismatch, expected ", modelConfig.default_depth_format, " got ", frameFormat)); } } @@ -290,13 +286,13 @@ void validateDepthFrame(std::shared_ptr depth, vsdk::Camera::raw_image encodeColorFrame(std::shared_ptr color) { vsdk::Camera::raw_image image; image.source_name = kColorSourceName; - + std::uint8_t* colorData = (std::uint8_t*)color->getData(); if (colorData == nullptr) { throw std::runtime_error("color data is null"); } uint32_t colorDataSize = color->dataSize(); - + if (color->getFormat() == OB_FORMAT_MJPG) { image.mime_type = kColorMimeTypeJPEG; image.bytes.assign(colorData, colorData + colorDataSize); @@ -306,13 +302,13 @@ vsdk::Camera::raw_image encodeColorFrame(std::shared_ptr color) { auto height = color->getStreamProfile()->as()->getHeight(); image.bytes = encoding::encode_to_png(colorData, width, height); } else { - throw std::runtime_error(formatError("unsupported color format: ", - ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()))); + throw std::runtime_error( + formatError("unsupported color format: ", ob::TypeHelper::convertOBFormatTypeToString(color->getFormat()))); } return image; } -} // anonymous namespace +} // anonymous namespace std::vector RGBPointsToPCD(std::shared_ptr frame, float scale) { int numPoints = frame->dataSize() / sizeof(OBColorPoint); @@ -610,7 +606,7 @@ void startDevice(std::string serialNumber, const OrbbecModelConfig* modelConfig) buffer << service_name << ": device " << serialNumber << " is not a supported camera (found: " << deviceName << ")"; throw std::invalid_argument(buffer.str()); } - + VIAM_SDK_LOG(info) << "Device " << serialNumber << " is supported: " << deviceName; std::unique_ptr& my_dev = search->second; @@ -636,80 +632,84 @@ void startDevice(std::string serialNumber, const OrbbecModelConfig* modelConfig) // Create the pipeline auto config = createHwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt, *modelConfig); if (config == nullptr) { - VIAM_SDK_LOG(warn) << "Device " << serialNumber << " does not support hardware depth-to-color alignment, trying software alignment"; - + VIAM_SDK_LOG(warn) << "Device " << serialNumber + << " does not support hardware depth-to-color alignment, trying software alignment"; + // Try to create a config with software alignment that respects user's requested resolution/format try { auto colorStreamProfiles = search->second->pipe->getStreamProfileList(OB_SENSOR_COLOR); auto depthStreamProfiles = search->second->pipe->getStreamProfileList(OB_SENSOR_DEPTH); - + if (colorStreamProfiles->getCount() > 0 && depthStreamProfiles->getCount() > 0) { config = std::make_shared(); - + // Find profiles that match the user's requested resolution and format auto colorSpCount = colorStreamProfiles->getCount(); auto depthSpCount = depthStreamProfiles->getCount(); - + std::shared_ptr colorProfile = nullptr; std::shared_ptr depthProfile = nullptr; - + // Look for matching color profile for (uint32_t i = 0; i < colorSpCount; i++) { auto profile = colorStreamProfiles->getProfile(i); auto vsp = profile->as(); - + bool resolutionMatch = true; bool formatMatch = true; - + // Check resolution if specified if (resolution_opt.has_value() && resolution_opt->color_resolution.has_value()) { resolutionMatch = (vsp->getWidth() == resolution_opt->color_resolution->width && - vsp->getHeight() == resolution_opt->color_resolution->height); + vsp->getHeight() == resolution_opt->color_resolution->height); } - + // Check format if specified if (format_opt.has_value() && format_opt->color_format.has_value()) { - formatMatch = (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == format_opt->color_format.value()); + formatMatch = + (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == format_opt->color_format.value()); } - + if (resolutionMatch && formatMatch) { colorProfile = profile; break; } } - + // Look for matching depth profile for (uint32_t i = 0; i < depthSpCount; i++) { auto profile = depthStreamProfiles->getProfile(i); auto vsp = profile->as(); - + bool resolutionMatch = true; bool formatMatch = true; - + // Check resolution if specified if (resolution_opt.has_value() && resolution_opt->depth_resolution.has_value()) { resolutionMatch = (vsp->getWidth() == resolution_opt->depth_resolution->width && - vsp->getHeight() == resolution_opt->depth_resolution->height); + vsp->getHeight() == resolution_opt->depth_resolution->height); } - + // Check format if specified if (format_opt.has_value() && format_opt->depth_format.has_value()) { - formatMatch = (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == format_opt->depth_format.value()); + formatMatch = + (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == format_opt->depth_format.value()); } - + if (resolutionMatch && formatMatch) { depthProfile = profile; break; } } - + // If no exact match found, throw an error since user specified requirements if (!colorProfile) { std::ostringstream buffer; buffer << service_name << ": device with serial number " << serialNumber << " does not support the requested color resolution/format: "; if (resolution_opt.has_value() && resolution_opt->color_resolution.has_value()) { - buffer << "resolution " << resolution_opt->color_resolution->width << "x" << resolution_opt->color_resolution->height; + buffer << "resolution " << resolution_opt->color_resolution->width << "x" + << resolution_opt->color_resolution->height; } if (format_opt.has_value() && format_opt->color_format.has_value()) { buffer << ", format " << format_opt->color_format.value(); @@ -721,24 +721,25 @@ void startDevice(std::string serialNumber, const OrbbecModelConfig* modelConfig) buffer << service_name << ": device with serial number " << serialNumber << " does not support the requested depth resolution/format: "; if (resolution_opt.has_value() && resolution_opt->depth_resolution.has_value()) { - buffer << "resolution " << resolution_opt->depth_resolution->width << "x" << resolution_opt->depth_resolution->height; + buffer << "resolution " << resolution_opt->depth_resolution->width << "x" + << resolution_opt->depth_resolution->height; } if (format_opt.has_value() && format_opt->depth_format.has_value()) { buffer << ", format " << format_opt->depth_format.value(); } throw std::runtime_error(buffer.str()); } - + config->enableStream(colorProfile); config->enableStream(depthProfile); config->setAlignMode(ALIGN_D2C_SW_MODE); // Use software alignment config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); - + auto colorVsp = colorProfile->as(); auto depthVsp = depthProfile->as(); - VIAM_SDK_LOG(info) << "Using software depth-to-color alignment for device " << serialNumber - << " with color " << colorVsp->getWidth() << "x" << colorVsp->getHeight() - << " and depth " << depthVsp->getWidth() << "x" << depthVsp->getHeight(); + VIAM_SDK_LOG(info) << "Using software depth-to-color alignment for device " << serialNumber << " with color " + << colorVsp->getWidth() << "x" << colorVsp->getHeight() << " and depth " << depthVsp->getWidth() + << "x" << depthVsp->getHeight(); } else { throw std::runtime_error("No stream profiles available"); } @@ -1064,7 +1065,7 @@ Orbbec::Orbbec(vsdk::Dependencies deps, vsdk::ResourceConfig cfg, std::shared_pt auto search = devices_by_serial().find(serial_number_); if (search != devices_by_serial().end()) { firmware_version_ = search->second->device->getDeviceInfo()->firmwareVersion(); - + // Detect model and set model_config_ std::shared_ptr deviceInfo = search->second->device->getDeviceInfo(); model_config_ = &OrbbecModelConfig::forDevice(deviceInfo->name()); @@ -1083,7 +1084,7 @@ Orbbec::~Orbbec() { const std::lock_guard lock(config_by_serial_mu()); if (config_by_serial().count(serial_number_) == 0) { VIAM_RESOURCE_LOG(error) << "Orbbec destructor: device with serial number " << serial_number_ - << " is not in config_by_serial, skipping erase"; + << " is not in config_by_serial, skipping erase"; } else { prev_serial_number = config_by_serial().at(serial_number_).serial_number; prev_resource_name = config_by_serial().at(serial_number_).resource_name; @@ -1175,7 +1176,7 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro fs = search->second; } std::shared_ptr color = fs->getFrame(OB_FRAME_COLOR); - + std::optional res_format_opt; { std::lock_guard lock(config_by_serial_mu()); @@ -1316,7 +1317,7 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte if (should_process_color) { color = fs->getFrame(OB_FRAME_COLOR); validateColorFrame(color, device_format_opt, *model_config_); - + vsdk::Camera::raw_image color_image = encodeColorFrame(color); response.images.emplace_back(std::move(color_image)); } @@ -1350,8 +1351,8 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte if (colorTS > 0 && depthTS > 0) { if (colorTS != depthTS) { VIAM_RESOURCE_LOG(info) << "color and depth timestamps differ, defaulting to " - "older of the two" - << "color timestamp was " << colorTS << " depth timestamp was " << depthTS; + "older of the two" + << "color timestamp was " << colorTS << " depth timestamp was " << depthTS; } // use the older of the two timestamps timestamp = (colorTS > depthTS) ? depthTS : colorTS; @@ -1581,7 +1582,7 @@ vsdk::Camera::point_cloud Orbbec::get_point_cloud(std::string mime_type, const v outfile_name << viam_module_data << "/pointcloud_" << timestamp << ".pcd"; } else { VIAM_RESOURCE_LOG(warn) << "VIAM_MODULE_DATA is set to " << viam_module_data - << " but is not a valid directory, using current working directory to store PCD file"; + << " but is not a valid directory, using current working directory to store PCD file"; outfile_name << "pointcloud_" << timestamp << ".pcd"; } } else { @@ -1650,20 +1651,20 @@ void registerDevice(std::string serialNumber, std::shared_ptr dev) { VIAM_SDK_LOG(info) << "starting " << serialNumber; std::shared_ptr pipe = std::make_shared(dev); pipe->enableFrameSync(); - + // Determine model configuration std::shared_ptr deviceInfo = dev->getDeviceInfo(); const OrbbecModelConfig* modelConfig = &OrbbecModelConfig::forDevice(deviceInfo->name()); - + std::shared_ptr config = createHwD2CAlignConfig(pipe, std::nullopt, std::nullopt, *modelConfig); if (config == nullptr) { VIAM_SDK_LOG(warn) << "Device " << serialNumber << " does not support hardware depth-to-color alignment, trying software alignment"; - + // Try to create a basic config without hardware alignment try { auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); - + if (colorStreamProfiles->getCount() > 0 && depthStreamProfiles->getCount() > 0) { config = std::make_shared(); config->enableStream(colorStreamProfiles->getProfile(0)); @@ -1763,8 +1764,8 @@ void deviceChangedCallback(const std::shared_ptr removedList, co } } catch (ob::Error& e) { VIAM_SDK_LOG(error) << "setDeviceChangedCallback\n" - << "function:" << e.getFunction() << "\nargs:" << e.getArgs() << "\nname:" << e.getName() << "\nmessage:" << e.what() - << "\ntype:" << e.getExceptionType() << std::endl; + << "function:" << e.getFunction() << "\nargs:" << e.getArgs() << "\nname:" << e.getName() + << "\nmessage:" << e.what() << "\ntype:" << e.getExceptionType() << std::endl; } } @@ -1785,14 +1786,14 @@ void startOrbbecSDK(ob::Context& ctx) { // Use SDK's native device discovery (works for both USB and network devices) std::shared_ptr devList = ctx.queryDeviceList(); int devCount = devList->getCount(); - + if (devCount == 0) { VIAM_SDK_LOG(warn) << "No Orbbec devices found"; return; } - + VIAM_SDK_LOG(info) << "Found " << devCount << " Orbbec devices"; - + for (size_t i = 0; i < devCount; i++) { try { // Get device information from DeviceList without triggering USB control transfers @@ -1802,16 +1803,15 @@ void startOrbbecSDK(ob::Context& ctx) { std::string ipAddress = devList->ipAddress(i); std::stringstream deviceInfoString; - deviceInfoString << "Device " << (i + 1) << " - Name: " << deviceName - << ", Serial: " << serialNumber + deviceInfoString << "Device " << (i + 1) << " - Name: " << deviceName << ", Serial: " << serialNumber << ", Connection: " << connectionType; if (!ipAddress.empty()) { deviceInfoString << ", IP: " << ipAddress; } VIAM_SDK_LOG(info) << deviceInfoString.str(); - + // Only create device if we can get the serial number - if (!serialNumber.empty()){ + if (!serialNumber.empty()) { try { std::shared_ptr dev = devList->getDevice(i); registerDevice(serialNumber, dev); @@ -1828,18 +1828,18 @@ void startOrbbecSDK(ob::Context& ctx) { } } } catch (ob::Error& e) { - VIAM_SDK_LOG(error) << "Failed to query Orbbec devices: " << e.what() - << " (function: " << e.getFunction() - << ", args: " << e.getArgs() - << ", name: " << e.getName() - << ", type: " << e.getExceptionType() << ")"; - VIAM_SDK_LOG(warn) << "Continuing without Orbbec devices - check network connectivity for Ethernet cameras or USB connection for USB cameras"; + VIAM_SDK_LOG(error) << "Failed to query Orbbec devices: " << e.what() << " (function: " << e.getFunction() + << ", args: " << e.getArgs() << ", name: " << e.getName() << ", type: " << e.getExceptionType() << ")"; + VIAM_SDK_LOG(warn) + << "Continuing without Orbbec devices - check network connectivity for Ethernet cameras or USB connection for USB cameras"; } catch (const std::exception& e) { VIAM_SDK_LOG(error) << "Failed to query Orbbec devices: " << e.what(); - VIAM_SDK_LOG(warn) << "Continuing without Orbbec devices - check network connectivity for Ethernet cameras or USB connection for USB cameras"; + VIAM_SDK_LOG(warn) + << "Continuing without Orbbec devices - check network connectivity for Ethernet cameras or USB connection for USB cameras"; } catch (...) { VIAM_SDK_LOG(error) << "Failed to query Orbbec devices: unknown error"; - VIAM_SDK_LOG(warn) << "Continuing without Orbbec devices - check network connectivity for Ethernet cameras or USB connection for USB cameras"; + VIAM_SDK_LOG(warn) + << "Continuing without Orbbec devices - check network connectivity for Ethernet cameras or USB connection for USB cameras"; } } // ORBBEC SDK DEVICE REGISTRY END diff --git a/src/module/orbbec.hpp b/src/module/orbbec.hpp index efc2f1fb..3f4f7db7 100644 --- a/src/module/orbbec.hpp +++ b/src/module/orbbec.hpp @@ -97,8 +97,8 @@ struct ObResourceConfig { }; struct OrbbecModelConfig { - std::string model_name; // "Astra 2" or "Gemini 335Le" - std::string viam_model_suffix; // "astra2" or "gemini_335le" + std::string model_name; // "Astra 2" or "Gemini 335Le" + std::string viam_model_suffix; // "astra2" or "gemini_335le" Resolution default_color_resolution; Resolution default_depth_resolution; std::optional firmware_url; @@ -108,10 +108,10 @@ struct OrbbecModelConfig { std::unordered_set supported_depth_formats; std::string default_color_format; std::string default_depth_format; - + // Get config for a device name static const OrbbecModelConfig& forDevice(const std::string& device_name); - + // Check if a device name is supported static bool isSupported(const std::string& device_name); }; @@ -160,7 +160,7 @@ class Orbbec final : public viam::sdk::Camera, public viam::sdk::Reconfigurable const OrbbecModelConfig* model_config_; static std::unique_ptr configure(viam::sdk::Dependencies deps, viam::sdk::ResourceConfig cfg); static void validate_sensor(std::pair const& sensor_pair, const OrbbecModelConfig& modelConfig); - + // Create a config for hardware depth-to-color alignment static std::shared_ptr createHwD2CAlignConfig(std::shared_ptr pipe, std::optional deviceRes, diff --git a/src/module/orbbec_firmware.cpp b/src/module/orbbec_firmware.cpp index b7c89de6..bcfe19d3 100644 --- a/src/module/orbbec_firmware.cpp +++ b/src/module/orbbec_firmware.cpp @@ -17,164 +17,167 @@ #include #include -#include #include +#include #include #include namespace orbbec { namespace firmware { - size_t writeFileCallback(void* contents, size_t size, size_t nmemb, void* userp) { - auto* buffer = static_cast*>(userp); - size_t totalSize = size * nmemb; - buffer->insert(buffer->end(), static_cast(contents), static_cast(contents) + totalSize); - return totalSize; +size_t writeFileCallback(void* contents, size_t size, size_t nmemb, void* userp) { + auto* buffer = static_cast*>(userp); + size_t totalSize = size * nmemb; + buffer->insert(buffer->end(), static_cast(contents), static_cast(contents) + totalSize); + return totalSize; +} + +template +struct Cleanup { + using pointer_type = std::tuple_element_t<0, boost::callable_traits::args_t>; + using value_type = std::remove_pointer_t; + + void operator()(pointer_type p) { + if (p != nullptr) { + cleanup_fp(p); + } + } +}; + +template +using CleanupPtr = std::unique_ptr::value_type, Cleanup>; + +void updateFirmware(std::unique_ptr& my_dev, + std::shared_ptr ctx, + const std::string& firmwareUrl, + viam::sdk::LogSource& logger) { +// On linux, orbbec recommends to set libuvc backend for firmware update +#if defined(__linux__) + ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC); +#endif + + CleanupPtr curl(curl_easy_init()); + if (!curl) { + throw std::invalid_argument("curl easy init failed"); + } + + // Download the firmware and write it to a buffer + std::vector zipBuffer; + curl_easy_setopt(curl.get(), CURLOPT_URL, firmwareUrl.c_str()); + curl_easy_setopt(curl.get(), CURLOPT_FOLLOWLOCATION, 1L); + curl_easy_setopt(curl.get(), CURLOPT_WRITEFUNCTION, writeFileCallback); + curl_easy_setopt(curl.get(), CURLOPT_WRITEDATA, &zipBuffer); + CURLcode res = curl_easy_perform(curl.get()); + if (res != CURLE_OK) { + std::ostringstream buffer; + buffer << "curl easy perform failed: " << curl_easy_strerror(res); + throw std::invalid_argument(buffer.str()); + } + + std::vector binData; + zip_error_t ziperror; + zip_error_init(&ziperror); + + zip_source_t* src = zip_source_buffer_create(zipBuffer.data(), zipBuffer.size(), 0, &ziperror); + if (!src) { + std::ostringstream buffer; + buffer << "failed to create zip buffer: " << zip_error_strerror(&ziperror); + throw std::runtime_error(buffer.str()); + } + + // Ensure src cleanup if zip_open fails + CleanupPtr srcCleanup(src); + + // If this succeeds, zip takes ownership of src, so src will be freed when zip_close is called. + zip_t* zip = zip_open_from_source(src, 0, &ziperror); + if (!zip) { + std::ostringstream buffer; + buffer << "failed to open zip from source: " << zip_error_strerror(&ziperror); + throw std::runtime_error(buffer.str()); + } + + srcCleanup.release(); + CleanupPtr zipCleanup(zip); + + if (zip_get_num_entries(zip, 0) != 1) { + throw std::runtime_error("unexpected number of files in firmware zip"); + } + + const char* fileName = zip_get_name(zip, 0, 0); + if (!fileName) { + throw std::runtime_error("couldn't get bin file name"); } - - template - struct Cleanup { - using pointer_type = std::tuple_element_t<0, boost::callable_traits::args_t>; - using value_type = std::remove_pointer_t; - - void operator()(pointer_type p) { - if (p != nullptr) { - cleanup_fp(p); - } + + CleanupPtr binFile(zip_fopen(zip, fileName, 0)); + if (!binFile) { + throw std::runtime_error("failed to open the firmware bin file"); + } + + zip_stat_t stats; + zip_stat_init(&stats); + if (zip_stat(zip, fileName, 0, &stats) != 0) { + throw std::invalid_argument("failed to stat file"); + } + + binData.resize(stats.size); + zip_int64_t bytesRead = zip_fread(binFile.get(), binData.data(), stats.size); + if (bytesRead == -1) { + zip_error_t* err = zip_file_get_error(binFile.get()); + std::ostringstream buffer; + buffer << "failed to read bin: " << zip_error_strerror(err); + throw std::runtime_error(buffer.str()); + } + + if (bytesRead != stats.size) { + std::ostringstream buffer; + buffer << "failed to fully read binary file, file size: " << stats.size << "bytes read: " << bytesRead; + throw std::runtime_error(buffer.str()); + } + + auto firmwareUpdateCallback = [](OBFwUpdateState state, const char* message, uint8_t percent) { + switch (state) { + case STAT_VERIFY_SUCCESS: + std::cout << "Image file verification success\n"; + break; + case STAT_FILE_TRANSFER: + std::cout << "File transfer in progress\n"; + break; + case STAT_DONE: + std::cout << "Update completed\n"; + break; + case STAT_IN_PROGRESS: + std::cout << "Upgrade in progress\n"; + break; + case STAT_START: + std::cout << "Starting the upgrade\n"; + break; + case STAT_VERIFY_IMAGE: + std::cout << "Verifying image file\n"; + break; + default: + std::cerr << "Unknown status or error\n"; + break; } + std::cout << "Firmware update in progress: " << message << " upgrade " << static_cast(percent) << "% complete\n"; }; - - template - using CleanupPtr = std::unique_ptr::value_type, Cleanup>; - - void updateFirmware(std::unique_ptr& my_dev, std::shared_ptr ctx, const std::string& firmwareUrl, viam::sdk::LogSource &logger) { - // On linux, orbbec recommends to set libuvc backend for firmware update - #if defined(__linux__) - ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC); - #endif - - CleanupPtr curl(curl_easy_init()); - if (!curl) { - throw std::invalid_argument("curl easy init failed"); - } - - // Download the firmware and write it to a buffer - std::vector zipBuffer; - curl_easy_setopt(curl.get(), CURLOPT_URL, firmwareUrl.c_str()); - curl_easy_setopt(curl.get(), CURLOPT_FOLLOWLOCATION, 1L); - curl_easy_setopt(curl.get(), CURLOPT_WRITEFUNCTION, writeFileCallback); - curl_easy_setopt(curl.get(), CURLOPT_WRITEDATA, &zipBuffer); - CURLcode res = curl_easy_perform(curl.get()); - if (res != CURLE_OK) { - std::ostringstream buffer; - buffer << "curl easy perform failed: " << curl_easy_strerror(res); - throw std::invalid_argument(buffer.str()); - } - - std::vector binData; - zip_error_t ziperror; - zip_error_init(&ziperror); - - zip_source_t* src = zip_source_buffer_create(zipBuffer.data(), zipBuffer.size(), 0, &ziperror); - if (!src) { - std::ostringstream buffer; - buffer << "failed to create zip buffer: " << zip_error_strerror(&ziperror); - throw std::runtime_error(buffer.str()); - } - - // Ensure src cleanup if zip_open fails - CleanupPtr srcCleanup(src); - - // If this succeeds, zip takes ownership of src, so src will be freed when zip_close is called. - zip_t* zip = zip_open_from_source(src, 0, &ziperror); - if (!zip) { - std::ostringstream buffer; - buffer << "failed to open zip from source: " << zip_error_strerror(&ziperror); - throw std::runtime_error(buffer.str()); - } - - srcCleanup.release(); - CleanupPtr zipCleanup(zip); - - if (zip_get_num_entries(zip, 0) != 1) { - throw std::runtime_error("unexpected number of files in firmware zip"); - } - - const char* fileName = zip_get_name(zip, 0, 0); - if (!fileName) { - throw std::runtime_error("couldn't get bin file name"); - } - - CleanupPtr binFile(zip_fopen(zip, fileName, 0)); - if (!binFile) { - throw std::runtime_error("failed to open the firmware bin file"); - } - - zip_stat_t stats; - zip_stat_init(&stats); - if (zip_stat(zip, fileName, 0, &stats) != 0) { - throw std::invalid_argument("failed to stat file"); - } - - binData.resize(stats.size); - zip_int64_t bytesRead = zip_fread(binFile.get(), binData.data(), stats.size); - if (bytesRead == -1) { - zip_error_t* err = zip_file_get_error(binFile.get()); - std::ostringstream buffer; - buffer << "failed to read bin: " << zip_error_strerror(err); - throw std::runtime_error(buffer.str()); - } - - if (bytesRead != stats.size) { - std::ostringstream buffer; - buffer << "failed to fully read binary file, file size: " << stats.size << "bytes read: " << bytesRead; - throw std::runtime_error(buffer.str()); - } - - auto firmwareUpdateCallback = [](OBFwUpdateState state, const char* message, uint8_t percent) { - switch (state) { - case STAT_VERIFY_SUCCESS: - std::cout << "Image file verification success\n"; - break; - case STAT_FILE_TRANSFER: - std::cout << "File transfer in progress\n"; - break; - case STAT_DONE: - std::cout << "Update completed\n"; - break; - case STAT_IN_PROGRESS: - std::cout << "Upgrade in progress\n"; - break; - case STAT_START: - std::cout << "Starting the upgrade\n"; - break; - case STAT_VERIFY_IMAGE: - std::cout << "Verifying image file\n"; - break; - default: - std::cerr << "Unknown status or error\n"; - break; - } - std::cout << "Firmware update in progress: " << message << " upgrade " << static_cast(percent) << "% complete\n"; - }; - - bool executeAsync = false; - try { - my_dev->device->updateFirmwareFromData(binData.data(), binData.size(), std::move(firmwareUpdateCallback), executeAsync); - VIAM_SDK_LOG_IMPL(logger, info) << "firmware update successful!"; - } catch (...) { - VIAM_SDK_LOG_IMPL(logger, error) << "firmware update failed!"; - // Reset UVC backend type before re-throwing - #if defined(__linux__) - ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_AUTO); - #endif - throw; - } - - // Reset UVC backend type on success - #if defined(__linux__) + + bool executeAsync = false; + try { + my_dev->device->updateFirmwareFromData(binData.data(), binData.size(), std::move(firmwareUpdateCallback), executeAsync); + VIAM_SDK_LOG_IMPL(logger, info) << "firmware update successful!"; + } catch (...) { + VIAM_SDK_LOG_IMPL(logger, error) << "firmware update failed!"; + // Reset UVC backend type before re-throwing +#if defined(__linux__) ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_AUTO); - #endif +#endif + throw; } -} // namespace firmware -} // namespace orbbec \ No newline at end of file + + // Reset UVC backend type on success +#if defined(__linux__) + ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_AUTO); +#endif +} +} // namespace firmware +} // namespace orbbec \ No newline at end of file diff --git a/src/module/orbbec_firmware.hpp b/src/module/orbbec_firmware.hpp index 86686427..97421c9a 100644 --- a/src/module/orbbec_firmware.hpp +++ b/src/module/orbbec_firmware.hpp @@ -32,10 +32,10 @@ namespace firmware { size_t writeFileCallback(void* contents, size_t size, size_t nmemb, void* userp); // Main firmware update function -void updateFirmware(std::unique_ptr& my_dev, - std::shared_ptr ctx, - const std::string& firmware_url, - viam::sdk::LogSource &logger); +void updateFirmware(std::unique_ptr& my_dev, + std::shared_ptr ctx, + const std::string& firmware_url, + viam::sdk::LogSource& logger); -} // namespace firmware -} // namespace orbbec \ No newline at end of file +} // namespace firmware +} // namespace orbbec \ No newline at end of file diff --git a/src/module/orbbec_windows_registry.cpp b/src/module/orbbec_windows_registry.cpp index e52340aa..004ebead 100644 --- a/src/module/orbbec_windows_registry.cpp +++ b/src/module/orbbec_windows_registry.cpp @@ -16,9 +16,9 @@ #ifdef _WIN32 #include +#include #include #include -#include #include #include @@ -131,7 +131,7 @@ void setupWindowsDeviceRegistry(std::shared_ptr device) { } } -} // namespace windows_registry -} // namespace orbbec +} // namespace windows_registry +} // namespace orbbec -#endif // _WIN32 +#endif // _WIN32 diff --git a/src/module/orbbec_windows_registry.hpp b/src/module/orbbec_windows_registry.hpp index 9c02d641..b182ed35 100644 --- a/src/module/orbbec_windows_registry.hpp +++ b/src/module/orbbec_windows_registry.hpp @@ -29,5 +29,5 @@ std::string uint16ToHex(uint16_t value); // Setup Windows device registry for Orbbec cameras void setupWindowsDeviceRegistry(std::shared_ptr device); -} // namespace windows_registry -} // namespace orbbec +} // namespace windows_registry +} // namespace orbbec From d70a714d18cc378bc5e67d70c84c5775296007b4 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Wed, 29 Oct 2025 15:06:30 -0400 Subject: [PATCH 24/33] [RSDK-12362] some warn logs are now errors, removing raw pointers for std::optional when appropriate --- src/module/discovery.cpp | 21 +++++------ src/module/main.cpp | 1 + src/module/orbbec.cpp | 80 +++++++++++++++++++++++++++------------- src/module/orbbec.hpp | 4 +- 4 files changed, 67 insertions(+), 39 deletions(-) diff --git a/src/module/discovery.cpp b/src/module/discovery.cpp index da3e0aa8..ad394595 100644 --- a/src/module/discovery.cpp +++ b/src/module/discovery.cpp @@ -17,9 +17,9 @@ std::vector OrbbecDiscovery::discover_resources(const vsdk ob_ctx_->enableNetDeviceEnumeration(true); VIAM_RESOURCE_LOG(info) << "Enabled network device enumeration for Ethernet cameras"; } catch (ob::Error& e) { - VIAM_RESOURCE_LOG(warn) << "Failed to enable network device enumeration: " << e.what(); + VIAM_RESOURCE_LOG(error) << "Failed to enable network device enumeration: " << e.what(); } catch (const std::exception& e) { - VIAM_RESOURCE_LOG(warn) << "Failed to enable network device enumeration: " << e.what(); + VIAM_RESOURCE_LOG(error) << "Failed to enable network device enumeration: " << e.what(); } try { @@ -58,13 +58,12 @@ std::vector OrbbecDiscovery::discover_resources(const vsdk // Detect model and create appropriate resource std::string viamModelSuffix; - try { - const auto& modelConfig = orbbec::OrbbecModelConfig::forDevice(deviceName); - viamModelSuffix = modelConfig.viam_model_suffix; - } catch (const std::runtime_error& e) { - VIAM_RESOURCE_LOG(warn) << "Skipping unsupported camera: " << deviceName; + std::optional modelConfig = orbbec::OrbbecModelConfig::forDevice(deviceName); + if (!modelConfig.has_value()) { + VIAM_RESOURCE_LOG(error) << "Failed to determine model configuration for device " << deviceName; continue; } + viamModelSuffix = modelConfig->viam_model_suffix; vsdk::ResourceConfig config("camera", std::move(name.str()), @@ -77,20 +76,20 @@ std::vector OrbbecDiscovery::discover_resources(const vsdk VIAM_RESOURCE_LOG(info) << "Successfully configured device " << (i + 1) << " with serial: " << serialNumber; } catch (ob::Error& deviceError) { - VIAM_RESOURCE_LOG(warn) << "Failed to get device info for device " << (i + 1) << ": " << deviceError.what(); + VIAM_RESOURCE_LOG(error) << "Failed to get device info for device " << (i + 1) << ": " << deviceError.what(); // Continue with other devices even if one fails } } } catch (ob::Error& e) { VIAM_RESOURCE_LOG(error) << "Failed to discover Orbbec devices: " << e.what() << " (function: " << e.getFunction() << ", args: " << e.getArgs() << ", name: " << e.getName() << ", type: " << e.getExceptionType() << ")"; - VIAM_RESOURCE_LOG(warn) << "Discovery failed - check network connectivity for Ethernet cameras or USB connection for USB cameras"; + VIAM_RESOURCE_LOG(error) << "Discovery failed - check network connectivity for Ethernet cameras or USB connection for USB cameras"; } catch (const std::exception& e) { VIAM_RESOURCE_LOG(error) << "Failed to discover Orbbec devices: " << e.what(); - VIAM_RESOURCE_LOG(warn) << "Discovery failed - check network connectivity for Ethernet cameras or USB connection for USB cameras"; + VIAM_RESOURCE_LOG(error) << "Discovery failed - check network connectivity for Ethernet cameras or USB connection for USB cameras"; } catch (...) { VIAM_RESOURCE_LOG(error) << "Failed to discover Orbbec devices: unknown error"; - VIAM_RESOURCE_LOG(warn) << "Discovery failed - check network connectivity for Ethernet cameras or USB connection for USB cameras"; + VIAM_RESOURCE_LOG(error) << "Discovery failed - check network connectivity for Ethernet cameras or USB connection for USB cameras"; } return configs; diff --git a/src/module/main.cpp b/src/module/main.cpp index 91900193..2800130d 100644 --- a/src/module/main.cpp +++ b/src/module/main.cpp @@ -35,6 +35,7 @@ std::vector> create_all_model_registrat return std::make_unique(std::move(deps), std::move(config), ctx); })); + // Return the registrations return registrations; } diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 0a95e3d8..cc08c349 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -65,7 +65,7 @@ const uint64_t maxFrameAgeUs = 1e6; // time until a frame is considered stale, // Model configurations namespace { -const OrbbecModelConfig ASTRA2_CONFIG{ +static const OrbbecModelConfig ASTRA2_CONFIG{ "Astra 2", // model_name "astra2", // viam_model_suffix {1280, 720}, // default_color_resolution @@ -84,7 +84,7 @@ const OrbbecModelConfig ASTRA2_CONFIG{ "Y16" // default_depth_format }; -const OrbbecModelConfig GEMINI_335LE_CONFIG{ +static const OrbbecModelConfig GEMINI_335LE_CONFIG{ "Gemini 335Le", // model_name "gemini_335le", // viam_model_suffix {1280, 800}, // default_color_resolution @@ -102,10 +102,10 @@ const OrbbecModelConfig GEMINI_335LE_CONFIG{ }; } // namespace -const OrbbecModelConfig& OrbbecModelConfig::forDevice(const std::string& device_name) { - if (device_name.find("Astra2") != std::string::npos || device_name.find("Astra 2") != std::string::npos) +std::optional OrbbecModelConfig::forDevice(const std::string& device_name) { + if (device_name.find(ASTRA2_CONFIG.model_name) != std::string::npos) return ASTRA2_CONFIG; - if (device_name.find("Gemini 335Le") != std::string::npos) + if (device_name.find(GEMINI_335LE_CONFIG.model_name) != std::string::npos) return GEMINI_335LE_CONFIG; throw std::runtime_error("Unsupported Orbbec camera model: " + device_name); } @@ -588,7 +588,7 @@ auto frameCallback(const std::string& serialNumber) { }; } -void startDevice(std::string serialNumber, const OrbbecModelConfig* modelConfig) { +void startDevice(std::string serialNumber, std::optional modelConfig) { VIAM_SDK_LOG(info) << service_name << ": starting device " << serialNumber; std::lock_guard lock(devices_by_serial_mu()); auto search = devices_by_serial().find(serialNumber); @@ -1052,14 +1052,6 @@ Orbbec::Orbbec(vsdk::Dependencies deps, vsdk::ResourceConfig cfg, std::shared_pt applyExperimentalConfig(my_dev, cfg.attributes()); } - startDevice(serial_number_, model_config_); - { - std::lock_guard lock(serial_by_resource_mu()); - serial_by_resource()[config->resource_name] = serial_number_; - } - - // set firmware version member variable - { std::lock_guard lock(devices_by_serial_mu()); auto search = devices_by_serial().find(serial_number_); @@ -1068,11 +1060,21 @@ Orbbec::Orbbec(vsdk::Dependencies deps, vsdk::ResourceConfig cfg, std::shared_pt // Detect model and set model_config_ std::shared_ptr deviceInfo = search->second->device->getDeviceInfo(); - model_config_ = &OrbbecModelConfig::forDevice(deviceInfo->name()); + model_config_ = OrbbecModelConfig::forDevice(deviceInfo->name()); VIAM_SDK_LOG(info) << "Detected model: " << model_config_->model_name; } } + if (!model_config_.has_value()) { + throw std::runtime_error("Failed to detect Orbbec model configuration"); + } + + startDevice(serial_number_, model_config_); + { + std::lock_guard lock(serial_by_resource_mu()); + serial_by_resource()[config->resource_name] = serial_number_; + } + VIAM_RESOURCE_LOG(info) << "Orbbec constructor end " << serial_number_; } @@ -1135,12 +1137,6 @@ void Orbbec::reconfigure(const vsdk::Dependencies& deps, const vsdk::ResourceCon frame_set_by_serial().erase(prev_serial_number); } - startDevice(new_serial_number, model_config_); - { - std::lock_guard lock(serial_by_resource_mu()); - serial_by_resource()[new_resource_name] = new_serial_number; - } - // set firmware version member variable and apply experimental config { std::lock_guard lock_serial(serial_number_mu_); @@ -1148,10 +1144,25 @@ void Orbbec::reconfigure(const vsdk::Dependencies& deps, const vsdk::ResourceCon auto search = devices_by_serial().find(serial_number_); if (search != devices_by_serial().end()) { firmware_version_ = search->second->device->getDeviceInfo()->firmwareVersion(); + + // Detect model and set model_config_ + std::shared_ptr deviceInfo = search->second->device->getDeviceInfo(); + model_config_ = OrbbecModelConfig::forDevice(deviceInfo->name()); + std::unique_ptr& my_dev = search->second; applyExperimentalConfig(my_dev, cfg.attributes()); } } + + if (!model_config_.has_value()) { + throw std::runtime_error("Failed to detect Orbbec model configuration during reconfigure"); + } + + startDevice(new_serial_number, model_config_); + { + std::lock_guard lock(serial_by_resource_mu()); + serial_by_resource()[new_resource_name] = new_serial_number; + } VIAM_RESOURCE_LOG(info) << "[reconfigure] Orbbec reconfigure end"; } @@ -1164,7 +1175,9 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro serial_number = serial_number_; } - checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); + if (model_config_.has_value()) { + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); + } std::shared_ptr fs = nullptr; { @@ -1269,7 +1282,9 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte serial_number = serial_number_; } - checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); + if (model_config_.has_value()) { + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); + } std::shared_ptr fs = nullptr; { @@ -1394,6 +1409,9 @@ vsdk::ProtoStruct Orbbec::do_command(const vsdk::ProtoStruct& command) { if (key == firmware_key) { VIAM_RESOURCE_LOG(info) << "Received firmware update command"; vsdk::ProtoStruct resp = viam::sdk::ProtoStruct{}; + if (!model_config_.has_value()) { + throw std::runtime_error("Model configuration not available for firmware update"); + } if (firmware_version_.find(model_config_->min_firmware_version) != std::string::npos) { std::ostringstream buffer; buffer << "Device firmware already on version " << model_config_->min_firmware_version; @@ -1513,7 +1531,9 @@ vsdk::Camera::point_cloud Orbbec::get_point_cloud(std::string mime_type, const v serial_number = serial_number_; } - checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); + if (model_config_.has_value()) { + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); + } std::shared_ptr fs = nullptr; { @@ -1654,7 +1674,11 @@ void registerDevice(std::string serialNumber, std::shared_ptr dev) { // Determine model configuration std::shared_ptr deviceInfo = dev->getDeviceInfo(); - const OrbbecModelConfig* modelConfig = &OrbbecModelConfig::forDevice(deviceInfo->name()); + std::optional modelConfig = OrbbecModelConfig::forDevice(deviceInfo->name()); + if (!modelConfig.has_value()) { + VIAM_SDK_LOG(error) << "Failed to determine model configuration for device " << serialNumber; + return; + } std::shared_ptr config = createHwD2CAlignConfig(pipe, std::nullopt, std::nullopt, *modelConfig); if (config == nullptr) { @@ -1755,7 +1779,11 @@ void deviceChangedCallback(const std::shared_ptr removedList, co for (auto& [resource_name, serial_number] : serial_by_resource()) { if (serial_number == info->getSerialNumber()) { // Determine model configuration - const OrbbecModelConfig* modelConfig = &OrbbecModelConfig::forDevice(info->name()); + std::optional modelConfig = OrbbecModelConfig::forDevice(info->name()); + if (!modelConfig.has_value()) { + VIAM_SDK_LOG(error) << "Failed to determine model configuration for device " << serial_number; + continue; + } startDevice(serial_number, modelConfig); serial_by_resource()[resource_name] = serial_number; } diff --git a/src/module/orbbec.hpp b/src/module/orbbec.hpp index 3f4f7db7..615f6970 100644 --- a/src/module/orbbec.hpp +++ b/src/module/orbbec.hpp @@ -110,7 +110,7 @@ struct OrbbecModelConfig { std::string default_depth_format; // Get config for a device name - static const OrbbecModelConfig& forDevice(const std::string& device_name); + static std::optional forDevice(const std::string& device_name); // Check if a device name is supported static bool isSupported(const std::string& device_name); @@ -157,7 +157,7 @@ class Orbbec final : public viam::sdk::Camera, public viam::sdk::Reconfigurable std::string firmware_version_; std::mutex serial_number_mu_; std::string serial_number_; - const OrbbecModelConfig* model_config_; + std::optional model_config_; static std::unique_ptr configure(viam::sdk::Dependencies deps, viam::sdk::ResourceConfig cfg); static void validate_sensor(std::pair const& sensor_pair, const OrbbecModelConfig& modelConfig); From 6f978bb21831da00713d10fc5775c2f30bf71ae6 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Wed, 29 Oct 2025 16:40:09 -0400 Subject: [PATCH 25/33] [RSDK-12362] Take the fix the Orbbec name on the configs so that they can be used to detect Orbbec cameras directly --- src/module/orbbec.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index cc08c349..26b0cd81 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -66,7 +66,7 @@ const uint64_t maxFrameAgeUs = 1e6; // time until a frame is considered stale, // Model configurations namespace { static const OrbbecModelConfig ASTRA2_CONFIG{ - "Astra 2", // model_name + "Orbbec Astra2", // model_name "astra2", // viam_model_suffix {1280, 720}, // default_color_resolution {1600, 1200}, // default_depth_resolution @@ -85,7 +85,7 @@ static const OrbbecModelConfig ASTRA2_CONFIG{ }; static const OrbbecModelConfig GEMINI_335LE_CONFIG{ - "Gemini 335Le", // model_name + "Orbbec Gemini 335Le", // model_name "gemini_335le", // viam_model_suffix {1280, 800}, // default_color_resolution {1280, 800}, // default_depth_resolution From fa15ab414fc8e55d7bc388208609f262ffb3090c Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Wed, 29 Oct 2025 18:21:56 -0400 Subject: [PATCH 26/33] [RSDK-12362] Orbbec devices can have mutiple names, accounting for that here --- src/module/orbbec.cpp | 35 ++++++++++++++++++++++------------- src/module/orbbec.hpp | 2 +- 2 files changed, 23 insertions(+), 14 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 26b0cd81..701dc1af 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -66,7 +66,7 @@ const uint64_t maxFrameAgeUs = 1e6; // time until a frame is considered stale, // Model configurations namespace { static const OrbbecModelConfig ASTRA2_CONFIG{ - "Orbbec Astra2", // model_name + {"Orbbec Astra 2", "Orbbec Astra2"}, // model_names "astra2", // viam_model_suffix {1280, 720}, // default_color_resolution {1600, 1200}, // default_depth_resolution @@ -85,7 +85,7 @@ static const OrbbecModelConfig ASTRA2_CONFIG{ }; static const OrbbecModelConfig GEMINI_335LE_CONFIG{ - "Orbbec Gemini 335Le", // model_name + {"Orbbec Gemini 335Le"}, // model_names "gemini_335le", // viam_model_suffix {1280, 800}, // default_color_resolution {1280, 800}, // default_depth_resolution @@ -100,19 +100,28 @@ static const OrbbecModelConfig GEMINI_335LE_CONFIG{ "MJPG", // default_color_format "Y16" // default_depth_format }; + +static const std::vector all_model_configs = {ASTRA2_CONFIG, GEMINI_335LE_CONFIG}; } // namespace std::optional OrbbecModelConfig::forDevice(const std::string& device_name) { - if (device_name.find(ASTRA2_CONFIG.model_name) != std::string::npos) - return ASTRA2_CONFIG; - if (device_name.find(GEMINI_335LE_CONFIG.model_name) != std::string::npos) - return GEMINI_335LE_CONFIG; - throw std::runtime_error("Unsupported Orbbec camera model: " + device_name); + VIAM_SDK_LOG(debug) << "OrbbecModelConfig::forDevice called with device_name: '" << device_name << "'"; + + // Check each model config + for (const auto& config : all_model_configs) { + // First, try exact match on any model name in the set + if (config.model_names.count(device_name) != 0) { + VIAM_SDK_LOG(debug) << "Found exact match for device_name"; + return config; + } + } + + VIAM_SDK_LOG(debug) << "No match found for device_name"; + return std::nullopt; } bool OrbbecModelConfig::isSupported(const std::string& device_name) { - return (device_name.find("Astra2") != std::string::npos || device_name.find("Astra 2") != std::string::npos || - device_name.find("Gemini 335Le") != std::string::npos); + return forDevice(device_name).has_value(); } // CONSTANTS END @@ -1061,7 +1070,7 @@ Orbbec::Orbbec(vsdk::Dependencies deps, vsdk::ResourceConfig cfg, std::shared_pt // Detect model and set model_config_ std::shared_ptr deviceInfo = search->second->device->getDeviceInfo(); model_config_ = OrbbecModelConfig::forDevice(deviceInfo->name()); - VIAM_SDK_LOG(info) << "Detected model: " << model_config_->model_name; + VIAM_SDK_LOG(info) << "Detected model: " << model_config_->viam_model_suffix; } } @@ -1176,7 +1185,7 @@ vsdk::Camera::raw_image Orbbec::get_image(std::string mime_type, const vsdk::Pro } if (model_config_.has_value()) { - checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->viam_model_suffix); } std::shared_ptr fs = nullptr; @@ -1283,7 +1292,7 @@ vsdk::Camera::image_collection Orbbec::get_images(std::vector filte } if (model_config_.has_value()) { - checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->viam_model_suffix); } std::shared_ptr fs = nullptr; @@ -1532,7 +1541,7 @@ vsdk::Camera::point_cloud Orbbec::get_point_cloud(std::string mime_type, const v } if (model_config_.has_value()) { - checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->model_name); + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->viam_model_suffix); } std::shared_ptr fs = nullptr; diff --git a/src/module/orbbec.hpp b/src/module/orbbec.hpp index 615f6970..684c288b 100644 --- a/src/module/orbbec.hpp +++ b/src/module/orbbec.hpp @@ -97,7 +97,7 @@ struct ObResourceConfig { }; struct OrbbecModelConfig { - std::string model_name; // "Astra 2" or "Gemini 335Le" + std::unordered_set model_names; // "Astra 2" or "Gemini 335Le" std::string viam_model_suffix; // "astra2" or "gemini_335le" Resolution default_color_resolution; Resolution default_depth_resolution; From 7e9cbc7254a9ea29eb99bb6b62cd06b41ed857a6 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Wed, 29 Oct 2025 18:49:04 -0400 Subject: [PATCH 27/33] [RSDK-12362] Making lint happy --- src/module/orbbec.cpp | 6 +++--- src/module/orbbec.hpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 701dc1af..6a33969a 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -66,7 +66,7 @@ const uint64_t maxFrameAgeUs = 1e6; // time until a frame is considered stale, // Model configurations namespace { static const OrbbecModelConfig ASTRA2_CONFIG{ - {"Orbbec Astra 2", "Orbbec Astra2"}, // model_names + {"Orbbec Astra 2", "Orbbec Astra2"}, // model_names "astra2", // viam_model_suffix {1280, 720}, // default_color_resolution {1600, 1200}, // default_depth_resolution @@ -106,7 +106,7 @@ static const std::vector all_model_configs = {ASTRA2_CONFIG, std::optional OrbbecModelConfig::forDevice(const std::string& device_name) { VIAM_SDK_LOG(debug) << "OrbbecModelConfig::forDevice called with device_name: '" << device_name << "'"; - + // Check each model config for (const auto& config : all_model_configs) { // First, try exact match on any model name in the set @@ -115,7 +115,7 @@ std::optional OrbbecModelConfig::forDevice(const std::string& return config; } } - + VIAM_SDK_LOG(debug) << "No match found for device_name"; return std::nullopt; } diff --git a/src/module/orbbec.hpp b/src/module/orbbec.hpp index 684c288b..f0fca30b 100644 --- a/src/module/orbbec.hpp +++ b/src/module/orbbec.hpp @@ -97,8 +97,8 @@ struct ObResourceConfig { }; struct OrbbecModelConfig { - std::unordered_set model_names; // "Astra 2" or "Gemini 335Le" - std::string viam_model_suffix; // "astra2" or "gemini_335le" + std::unordered_set model_names; // "Astra 2" or "Gemini 335Le" + std::string viam_model_suffix; // "astra2" or "gemini_335le" Resolution default_color_resolution; Resolution default_depth_resolution; std::optional firmware_url; From bb6720a5864f566c6a932f47bd4fadc6b5621d01 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Thu, 30 Oct 2025 10:43:38 -0400 Subject: [PATCH 28/33] [RSDK-12362] Improving logic of the code per feedback from PR --- src/module/orbbec.cpp | 92 ++++++++----------------------------------- src/module/orbbec.hpp | 1 + 2 files changed, 17 insertions(+), 76 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 6a33969a..6f3c39fe 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -120,10 +120,6 @@ std::optional OrbbecModelConfig::forDevice(const std::string& return std::nullopt; } -bool OrbbecModelConfig::isSupported(const std::string& device_name) { - return forDevice(device_name).has_value(); -} - // CONSTANTS END // STRUCTS BEGIN @@ -597,7 +593,7 @@ auto frameCallback(const std::string& serialNumber) { }; } -void startDevice(std::string serialNumber, std::optional modelConfig) { +void startDevice(std::string serialNumber, OrbbecModelConfig const& modelConfig) { VIAM_SDK_LOG(info) << service_name << ": starting device " << serialNumber; std::lock_guard lock(devices_by_serial_mu()); auto search = devices_by_serial().find(serialNumber); @@ -610,11 +606,6 @@ void startDevice(std::string serialNumber, std::optional mode // Check if the device is a supported Orbbec camera std::shared_ptr deviceInfo = search->second->device->getDeviceInfo(); std::string deviceName = deviceInfo->name(); - if (!OrbbecModelConfig::isSupported(deviceName)) { - std::ostringstream buffer; - buffer << service_name << ": device " << serialNumber << " is not a supported camera (found: " << deviceName << ")"; - throw std::invalid_argument(buffer.str()); - } VIAM_SDK_LOG(info) << "Device " << serialNumber << " is supported: " << deviceName; @@ -639,7 +630,7 @@ void startDevice(std::string serialNumber, std::optional mode << (format_opt.has_value() ? format_opt->to_string() : "not specifed"); if (resolution_opt.has_value() || format_opt.has_value()) { // Create the pipeline - auto config = createHwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt, *modelConfig); + auto config = createHwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt, modelConfig); if (config == nullptr) { VIAM_SDK_LOG(warn) << "Device " << serialNumber << " does not support hardware depth-to-color alignment, trying software alignment"; @@ -882,7 +873,7 @@ void Orbbec::validate_sensor(std::pair const } } -std::vector Orbbec::validateAstra2(vsdk::ResourceConfig cfg) { +std::vector Orbbec::validateOrbbecModel(vsdk::ResourceConfig cfg, OrbbecModelConfig const& modelConfig) { auto attrs = cfg.attributes(); if (!attrs.count("serial_number")) { @@ -906,17 +897,17 @@ std::vector Orbbec::validateAstra2(vsdk::ResourceConfig cfg) { } if (sensors) { for (auto const& sensor_pair : *sensors) { - validate_sensor(sensor_pair, ASTRA2_CONFIG); + validate_sensor(sensor_pair, modelConfig); } } auto color_width_uint32 = static_cast(*sensors->at("color").get()->at("width").get()); auto color_height_uint32 = static_cast(*sensors->at("color").get()->at("height").get()); - if (ASTRA2_CONFIG.color_to_depth_supported_resolutions.count({color_width_uint32, color_height_uint32}) == 0) { + if (modelConfig.color_to_depth_supported_resolutions.count({color_width_uint32, color_height_uint32}) == 0) { std::ostringstream buffer; buffer << "color resolution must be one of: "; - for (const auto& res : ASTRA2_CONFIG.color_to_depth_supported_resolutions) { + for (const auto& res : modelConfig.color_to_depth_supported_resolutions) { buffer << "{" << res.first.to_string() << "} "; } VIAM_SDK_LOG(error) << buffer.str(); @@ -926,12 +917,12 @@ std::vector Orbbec::validateAstra2(vsdk::ResourceConfig cfg) { static_cast(*sensors->at("depth").get()->at("width").get()); auto depth_height_uint32 = static_cast(*sensors->at("depth").get()->at("height").get()); - if (ASTRA2_CONFIG.color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32}) + if (modelConfig.color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32}) .count({depth_width_uint32, depth_height_uint32}) == 0) { std::ostringstream buffer; buffer << "color/depth resolution combination not supported, for color resolution " << "{" << color_width_uint32 << ", " << color_height_uint32 << "}, depth resolution must be one of: "; - for (const auto& res : ASTRA2_CONFIG.color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32})) { + for (const auto& res : modelConfig.color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32})) { buffer << "{" << res.to_string() << "} "; } VIAM_SDK_LOG(error) << buffer.str(); @@ -942,64 +933,13 @@ std::vector Orbbec::validateAstra2(vsdk::ResourceConfig cfg) { return {}; } -std::vector Orbbec::validateGemini335Le(vsdk::ResourceConfig cfg) { - auto attrs = cfg.attributes(); - - if (!attrs.count("serial_number")) { - throw std::invalid_argument("serial_number is a required argument"); - } - - if (!attrs["serial_number"].get()) { - throw std::invalid_argument("serial_number must be a string"); - } - - // We already established this is a string, so it's safe to call this - std::string const serial = attrs["serial_number"].get_unchecked(); - if (serial.empty()) { - throw std::invalid_argument("serial_number must be a non-empty string"); - } - if (attrs.count("sensors")) { - auto sensors = attrs["sensors"].get(); - if (!sensors) { - throw std::invalid_argument("sensors must be a struct"); - } - if (sensors) { - for (auto const& sensor_pair : *sensors) { - validate_sensor(sensor_pair, GEMINI_335LE_CONFIG); - } - } - auto color_width_uint32 = - static_cast(*sensors->at("color").get()->at("width").get()); - auto color_height_uint32 = - static_cast(*sensors->at("color").get()->at("height").get()); - if (GEMINI_335LE_CONFIG.color_to_depth_supported_resolutions.count({color_width_uint32, color_height_uint32}) == 0) { - std::ostringstream buffer; - buffer << "color resolution must be one of: "; - for (const auto& res : GEMINI_335LE_CONFIG.color_to_depth_supported_resolutions) { - buffer << "{" << res.first.to_string() << "} "; - } - VIAM_SDK_LOG(error) << buffer.str(); - throw std::invalid_argument(buffer.str()); - } - auto depth_width_uint32 = - static_cast(*sensors->at("depth").get()->at("width").get()); - auto depth_height_uint32 = - static_cast(*sensors->at("depth").get()->at("height").get()); - if (GEMINI_335LE_CONFIG.color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32}) - .count({depth_width_uint32, depth_height_uint32}) == 0) { - std::ostringstream buffer; - buffer << "color/depth resolution combination not supported, for color resolution " << "{" << color_width_uint32 << ", " - << color_height_uint32 << "}, depth resolution must be one of: "; - for (const auto& res : GEMINI_335LE_CONFIG.color_to_depth_supported_resolutions.at({color_width_uint32, color_height_uint32})) { - buffer << "{" << res.to_string() << "} "; - } - VIAM_SDK_LOG(error) << buffer.str(); - throw std::invalid_argument(buffer.str()); - } - } +std::vector Orbbec::validateAstra2(vsdk::ResourceConfig cfg) { + return validateOrbbecModel(cfg, ASTRA2_CONFIG); +} - return {}; +std::vector Orbbec::validateGemini335Le(vsdk::ResourceConfig cfg) { + return validateOrbbecModel(cfg, GEMINI_335LE_CONFIG); } std::string getSerialNumber(const vsdk::ResourceConfig& cfg) { @@ -1078,7 +1018,7 @@ Orbbec::Orbbec(vsdk::Dependencies deps, vsdk::ResourceConfig cfg, std::shared_pt throw std::runtime_error("Failed to detect Orbbec model configuration"); } - startDevice(serial_number_, model_config_); + startDevice(serial_number_, model_config_.value()); { std::lock_guard lock(serial_by_resource_mu()); serial_by_resource()[config->resource_name] = serial_number_; @@ -1167,7 +1107,7 @@ void Orbbec::reconfigure(const vsdk::Dependencies& deps, const vsdk::ResourceCon throw std::runtime_error("Failed to detect Orbbec model configuration during reconfigure"); } - startDevice(new_serial_number, model_config_); + startDevice(new_serial_number, model_config_.value()); { std::lock_guard lock(serial_by_resource_mu()); serial_by_resource()[new_resource_name] = new_serial_number; @@ -1793,7 +1733,7 @@ void deviceChangedCallback(const std::shared_ptr removedList, co VIAM_SDK_LOG(error) << "Failed to determine model configuration for device " << serial_number; continue; } - startDevice(serial_number, modelConfig); + startDevice(serial_number, modelConfig.value()); serial_by_resource()[resource_name] = serial_number; } } diff --git a/src/module/orbbec.hpp b/src/module/orbbec.hpp index f0fca30b..de7e8994 100644 --- a/src/module/orbbec.hpp +++ b/src/module/orbbec.hpp @@ -148,6 +148,7 @@ class Orbbec final : public viam::sdk::Camera, public viam::sdk::Reconfigurable std::vector get_geometries(const viam::sdk::ProtoStruct& extra) override; static std::vector validateAstra2(viam::sdk::ResourceConfig cfg); static std::vector validateGemini335Le(viam::sdk::ResourceConfig cfg); + static std::vector validateOrbbecModel(viam::sdk::ResourceConfig cfg, OrbbecModelConfig const& modelConfig); static viam::sdk::GeometryConfig geometry; static viam::sdk::Model model_astra2; static viam::sdk::Model model_gemini_335le; From 6ff60dd66666e12b442598794291b2dc40439bc8 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Thu, 30 Oct 2025 11:05:28 -0400 Subject: [PATCH 29/33] [RSDK-12362] Modularizing the creation of Alignment config for SW and HW --- src/module/orbbec.cpp | 262 +++++++++++++++++++----------------------- 1 file changed, 118 insertions(+), 144 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 6f3c39fe..cce0b2c4 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -435,6 +435,115 @@ bool checkIfSupportHWD2CAlign(std::shared_ptr pipe, return false; } +// create a config for software depth-to-color alignment with specified resolution/format +std::shared_ptr createSwD2CAlignConfig(std::shared_ptr pipe, + std::optional deviceRes, + std::optional deviceFormat, + const OrbbecModelConfig& modelConfig) { + auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); + auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); + + if (colorStreamProfiles->getCount() == 0 || depthStreamProfiles->getCount() == 0) { + throw std::runtime_error("No stream profiles available"); + } + + auto config = std::make_shared(); + auto colorSpCount = colorStreamProfiles->getCount(); + auto depthSpCount = depthStreamProfiles->getCount(); + + std::shared_ptr colorProfile = nullptr; + std::shared_ptr depthProfile = nullptr; + + // Look for matching color profile + for (uint32_t i = 0; i < colorSpCount; i++) { + auto profile = colorStreamProfiles->getProfile(i); + auto vsp = profile->as(); + + bool resolutionMatch = true; + bool formatMatch = true; + + // Check resolution if specified + if (deviceRes.has_value() && deviceRes->color_resolution.has_value()) { + resolutionMatch = (vsp->getWidth() == deviceRes->color_resolution->width && + vsp->getHeight() == deviceRes->color_resolution->height); + } + + // Check format if specified + if (deviceFormat.has_value() && deviceFormat->color_format.has_value()) { + formatMatch = (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == deviceFormat->color_format.value()); + } + + if (resolutionMatch && formatMatch) { + colorProfile = profile; + break; + } + } + + // Look for matching depth profile + for (uint32_t i = 0; i < depthSpCount; i++) { + auto profile = depthStreamProfiles->getProfile(i); + auto vsp = profile->as(); + + bool resolutionMatch = true; + bool formatMatch = true; + + // Check resolution if specified + if (deviceRes.has_value() && deviceRes->depth_resolution.has_value()) { + resolutionMatch = (vsp->getWidth() == deviceRes->depth_resolution->width && + vsp->getHeight() == deviceRes->depth_resolution->height); + } + + // Check format if specified + if (deviceFormat.has_value() && deviceFormat->depth_format.has_value()) { + formatMatch = (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == deviceFormat->depth_format.value()); + } + + if (resolutionMatch && formatMatch) { + depthProfile = profile; + break; + } + } + + // If no exact match found, throw an error since user specified requirements + if (!colorProfile) { + std::ostringstream buffer; + buffer << service_name << " does not support the requested color resolution/format: "; + if (deviceRes.has_value() && deviceRes->color_resolution.has_value()) { + buffer << "resolution " << deviceRes->color_resolution->width << "x" + << deviceRes->color_resolution->height; + } + if (deviceFormat.has_value() && deviceFormat->color_format.has_value()) { + buffer << ", format " << deviceFormat->color_format.value(); + } + throw std::runtime_error(buffer.str()); + } + if (!depthProfile) { + std::ostringstream buffer; + buffer << service_name << " does not support the requested depth resolution/format: "; + if (deviceRes.has_value() && deviceRes->depth_resolution.has_value()) { + buffer << "resolution " << deviceRes->depth_resolution->width << "x" + << deviceRes->depth_resolution->height; + } + if (deviceFormat.has_value() && deviceFormat->depth_format.has_value()) { + buffer << ", format " << deviceFormat->depth_format.value(); + } + throw std::runtime_error(buffer.str()); + } + + config->enableStream(colorProfile); + config->enableStream(depthProfile); + config->setAlignMode(ALIGN_D2C_SW_MODE); // Use software alignment + config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); + + auto colorVsp = colorProfile->as(); + auto depthVsp = depthProfile->as(); + VIAM_SDK_LOG(info) << "Using software depth-to-color alignment with color " + << colorVsp->getWidth() << "x" << colorVsp->getHeight() << " and depth " << depthVsp->getWidth() + << "x" << depthVsp->getHeight(); + + return config; +} + // create a config for hardware depth-to-color alignment std::shared_ptr createHwD2CAlignConfig(std::shared_ptr pipe, std::optional deviceRes, @@ -629,161 +738,26 @@ void startDevice(std::string serialNumber, OrbbecModelConfig const& modelConfig) << (resolution_opt.has_value() ? resolution_opt->to_string() : "not specified, Format from config: ") << (format_opt.has_value() ? format_opt->to_string() : "not specifed"); if (resolution_opt.has_value() || format_opt.has_value()) { - // Create the pipeline + // Create the pipeline config auto config = createHwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt, modelConfig); if (config == nullptr) { VIAM_SDK_LOG(warn) << "Device " << serialNumber << " does not support hardware depth-to-color alignment, trying software alignment"; - - // Try to create a config with software alignment that respects user's requested resolution/format - try { - auto colorStreamProfiles = search->second->pipe->getStreamProfileList(OB_SENSOR_COLOR); - auto depthStreamProfiles = search->second->pipe->getStreamProfileList(OB_SENSOR_DEPTH); - - if (colorStreamProfiles->getCount() > 0 && depthStreamProfiles->getCount() > 0) { - config = std::make_shared(); - - // Find profiles that match the user's requested resolution and format - auto colorSpCount = colorStreamProfiles->getCount(); - auto depthSpCount = depthStreamProfiles->getCount(); - - std::shared_ptr colorProfile = nullptr; - std::shared_ptr depthProfile = nullptr; - - // Look for matching color profile - for (uint32_t i = 0; i < colorSpCount; i++) { - auto profile = colorStreamProfiles->getProfile(i); - auto vsp = profile->as(); - - bool resolutionMatch = true; - bool formatMatch = true; - - // Check resolution if specified - if (resolution_opt.has_value() && resolution_opt->color_resolution.has_value()) { - resolutionMatch = (vsp->getWidth() == resolution_opt->color_resolution->width && - vsp->getHeight() == resolution_opt->color_resolution->height); - } - - // Check format if specified - if (format_opt.has_value() && format_opt->color_format.has_value()) { - formatMatch = - (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == format_opt->color_format.value()); - } - - if (resolutionMatch && formatMatch) { - colorProfile = profile; - break; - } - } - - // Look for matching depth profile - for (uint32_t i = 0; i < depthSpCount; i++) { - auto profile = depthStreamProfiles->getProfile(i); - auto vsp = profile->as(); - - bool resolutionMatch = true; - bool formatMatch = true; - - // Check resolution if specified - if (resolution_opt.has_value() && resolution_opt->depth_resolution.has_value()) { - resolutionMatch = (vsp->getWidth() == resolution_opt->depth_resolution->width && - vsp->getHeight() == resolution_opt->depth_resolution->height); - } - - // Check format if specified - if (format_opt.has_value() && format_opt->depth_format.has_value()) { - formatMatch = - (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == format_opt->depth_format.value()); - } - - if (resolutionMatch && formatMatch) { - depthProfile = profile; - break; - } - } - - // If no exact match found, throw an error since user specified requirements - if (!colorProfile) { - std::ostringstream buffer; - buffer << service_name << ": device with serial number " << serialNumber - << " does not support the requested color resolution/format: "; - if (resolution_opt.has_value() && resolution_opt->color_resolution.has_value()) { - buffer << "resolution " << resolution_opt->color_resolution->width << "x" - << resolution_opt->color_resolution->height; - } - if (format_opt.has_value() && format_opt->color_format.has_value()) { - buffer << ", format " << format_opt->color_format.value(); - } - throw std::runtime_error(buffer.str()); - } - if (!depthProfile) { - std::ostringstream buffer; - buffer << service_name << ": device with serial number " << serialNumber - << " does not support the requested depth resolution/format: "; - if (resolution_opt.has_value() && resolution_opt->depth_resolution.has_value()) { - buffer << "resolution " << resolution_opt->depth_resolution->width << "x" - << resolution_opt->depth_resolution->height; - } - if (format_opt.has_value() && format_opt->depth_format.has_value()) { - buffer << ", format " << format_opt->depth_format.value(); - } - throw std::runtime_error(buffer.str()); - } - - config->enableStream(colorProfile); - config->enableStream(depthProfile); - config->setAlignMode(ALIGN_D2C_SW_MODE); // Use software alignment - config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); - - auto colorVsp = colorProfile->as(); - auto depthVsp = depthProfile->as(); - VIAM_SDK_LOG(info) << "Using software depth-to-color alignment for device " << serialNumber << " with color " - << colorVsp->getWidth() << "x" << colorVsp->getHeight() << " and depth " << depthVsp->getWidth() - << "x" << depthVsp->getHeight(); - } else { - throw std::runtime_error("No stream profiles available"); - } - } catch (const std::exception& e) { - std::ostringstream buffer; - buffer << service_name << ": device with serial number " << serialNumber - << " does not support hardware depth-to-color alignment with the requested parameters: resolution " - << (resolution_opt.has_value() ? resolution_opt->to_string() : "none") << ", " - << (format_opt.has_value() ? format_opt->to_string() : "none") << "\n"; - VIAM_SDK_LOG(error) << buffer.str(); - throw std::runtime_error(buffer.str()); + // Use software alignment with user-specified resolution/format + config = createSwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt, modelConfig); + if(config != nullptr) { + VIAM_SDK_LOG(info) << "Device " << serialNumber << " supports software depth-to-color alignment, using it"; } + } else { + VIAM_SDK_LOG(info) << "Device " << serialNumber << " supports hardware depth-to-color alignment, using it"; } my_dev->config = config; } } - + // Start the pipeline with the configuration my_dev->pipe->start(my_dev->config, frameCallback(serialNumber)); my_dev->started = true; - - // Ensure we start getting frames within 500ms, otherwise something is - // wrong with the pipeline and we should restart. - auto start_time = std::chrono::steady_clock::now(); - bool got_frame = false; - while (std::chrono::steady_clock::now() - start_time < std::chrono::milliseconds(500)) { - { - std::lock_guard lock(frame_set_by_serial_mu()); - if (frame_set_by_serial().count(serialNumber) > 0) { - got_frame = true; - break; - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - } - - if (!got_frame) { - VIAM_SDK_LOG(error) << "did not get frame within 500ms of starting device, resetting"; - my_dev->pipe->stop(); - my_dev->started = false; - my_dev->pipe->start(my_dev->config, frameCallback(serialNumber)); - my_dev->started = true; - } - - VIAM_SDK_LOG(info) << service_name << ": device started " << serialNumber; + VIAM_SDK_LOG(info) << "Started pipeline for device " << serialNumber; } void stopDevice(std::string serialNumber, std::string resourceName) { From ec79f87eeff1c9f1016a0e5ac17b6eb7167ed4b8 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Thu, 30 Oct 2025 11:53:06 -0400 Subject: [PATCH 30/33] [RSDK-12362] Modularizing startDevice and registerDevice by adding a new function configureDevice and is the only place to handle the device configuration, as the resolutions config logic was duplicated --- src/module/device_control.hpp | 2 +- src/module/orbbec.cpp | 179 +++++++++++++++++----------------- src/module/orbbec.hpp | 4 +- 3 files changed, 91 insertions(+), 94 deletions(-) diff --git a/src/module/device_control.hpp b/src/module/device_control.hpp index 837bf5a1..9be6465f 100644 --- a/src/module/device_control.hpp +++ b/src/module/device_control.hpp @@ -657,7 +657,7 @@ viam::sdk::ProtoStruct createModuleConfig(std::unique_ptr& dev) { } viam::sdk::ProtoStruct result; - result["serial_number"] = dev->serial_number; + result["serial_number"] = dev->serialNumber; result["sensors"] = sensors; result["post_process_depth_filters"] = getPostProcessDepthFilters(dev->postProcessDepthFilters, "create_module_config")["create_module_config"]; diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index cce0b2c4..3a4c2742 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -702,6 +702,86 @@ auto frameCallback(const std::string& serialNumber) { }; } +void configureDevice(std::string serialNumber, OrbbecModelConfig const& modelConfig) { + VIAM_SDK_LOG(info) << "[configureDevice] Configuring device " << serialNumber; + std::lock_guard lock(devices_by_serial_mu()); + auto search = devices_by_serial().find(serialNumber); + if (search == devices_by_serial().end()) { + std::ostringstream buffer; + buffer << service_name << ": unable to configure undetected device " << serialNumber; + throw std::invalid_argument(buffer.str()); + } + + std::unique_ptr& my_dev = search->second; + + // Initialize fields if not already set + if (!my_dev->pipe) { + my_dev->pipe = std::make_shared(my_dev->device); + my_dev->pipe->enableFrameSync(); + } + if (!my_dev->pointCloudFilter) { + my_dev->pointCloudFilter = std::make_shared(); + my_dev->pointCloudFilter->setCreatePointFormat(OB_FORMAT_RGB_POINT); + } + if (!my_dev->align) { + my_dev->align = std::make_shared(OB_STREAM_COLOR); + } + if (my_dev->postProcessDepthFilters.empty()) { + auto depthSensor = my_dev->device->getSensor(OB_SENSOR_DEPTH); + if (depthSensor == nullptr) { + throw std::runtime_error("Current device does not have a depth sensor."); + } + my_dev->postProcessDepthFilters = depthSensor->createRecommendedFilters(); + my_dev->applyEnabledPostProcessDepthFilters = false; + } + + // Get user-specified resolution/format if any + std::optional resolution_opt; + std::optional format_opt; + { + std::lock_guard configLock(config_by_serial_mu()); + if (config_by_serial().count(serialNumber) > 0) { + resolution_opt = config_by_serial().at(serialNumber).device_resolution; + format_opt = config_by_serial().at(serialNumber).device_format; + VIAM_SDK_LOG(info) << "[configureDevice] Resolution from config: " + << (resolution_opt.has_value() ? resolution_opt->to_string() : "not specified, Format from config: ") + << (format_opt.has_value() ? format_opt->to_string() : "not specified"); + } else { + VIAM_SDK_LOG(info) << "[configureDevice] No custom config for " << serialNumber << ", using defaults"; + } + } + + // Create the pipeline config (with user settings if provided, otherwise defaults) + auto config = createHwD2CAlignConfig(my_dev->pipe, resolution_opt, format_opt, modelConfig); + if (config == nullptr) { + VIAM_SDK_LOG(warn) << "Device " << serialNumber + << " does not support hardware depth-to-color alignment, trying software alignment"; + // Use software alignment + if (resolution_opt.has_value() || format_opt.has_value()) { + config = createSwD2CAlignConfig(my_dev->pipe, resolution_opt, format_opt, modelConfig); + } else { + // Create basic config with first available profiles + auto colorStreamProfiles = my_dev->pipe->getStreamProfileList(OB_SENSOR_COLOR); + auto depthStreamProfiles = my_dev->pipe->getStreamProfileList(OB_SENSOR_DEPTH); + if (colorStreamProfiles->getCount() > 0 && depthStreamProfiles->getCount() > 0) { + config = std::make_shared(); + config->enableStream(colorStreamProfiles->getProfile(0)); + config->enableStream(depthStreamProfiles->getProfile(0)); + config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); + VIAM_SDK_LOG(info) << "Created basic config for device " << serialNumber; + } else { + throw std::runtime_error("No stream profiles available"); + } + } + if(config != nullptr) { + VIAM_SDK_LOG(info) << "Device " << serialNumber << " supports software depth-to-color alignment, using it"; + } + } else { + VIAM_SDK_LOG(info) << "Device " << serialNumber << " supports hardware depth-to-color alignment, using it"; + } + my_dev->config = config; +} + void startDevice(std::string serialNumber, OrbbecModelConfig const& modelConfig) { VIAM_SDK_LOG(info) << service_name << ": starting device " << serialNumber; std::lock_guard lock(devices_by_serial_mu()); @@ -712,7 +792,6 @@ void startDevice(std::string serialNumber, OrbbecModelConfig const& modelConfig) throw std::invalid_argument(buffer.str()); } - // Check if the device is a supported Orbbec camera std::shared_ptr deviceInfo = search->second->device->getDeviceInfo(); std::string deviceName = deviceInfo->name(); @@ -724,36 +803,7 @@ void startDevice(std::string serialNumber, OrbbecModelConfig const& modelConfig) buffer << service_name << ": device " << serialNumber << " is already started"; throw std::runtime_error(buffer.str()); } - VIAM_SDK_LOG(info) << "[startDevice] Configuring device resolution"; - { - std::lock_guard lock(config_by_serial_mu()); - if (config_by_serial().count(serialNumber) == 0) { - std::ostringstream buffer; - buffer << service_name << ": device with serial number " << serialNumber << " is not in config_by_serial, skipping start"; - throw std::runtime_error(buffer.str()); - } - auto resolution_opt = config_by_serial().at(serialNumber).device_resolution; - auto format_opt = config_by_serial().at(serialNumber).device_format; - VIAM_SDK_LOG(info) << "[startDevice] Resolution from config: " - << (resolution_opt.has_value() ? resolution_opt->to_string() : "not specified, Format from config: ") - << (format_opt.has_value() ? format_opt->to_string() : "not specifed"); - if (resolution_opt.has_value() || format_opt.has_value()) { - // Create the pipeline config - auto config = createHwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt, modelConfig); - if (config == nullptr) { - VIAM_SDK_LOG(warn) << "Device " << serialNumber - << " does not support hardware depth-to-color alignment, trying software alignment"; - // Use software alignment with user-specified resolution/format - config = createSwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt, modelConfig); - if(config != nullptr) { - VIAM_SDK_LOG(info) << "Device " << serialNumber << " supports software depth-to-color alignment, using it"; - } - } else { - VIAM_SDK_LOG(info) << "Device " << serialNumber << " supports hardware depth-to-color alignment, using it"; - } - my_dev->config = config; - } - } + // Start the pipeline with the configuration my_dev->pipe->start(my_dev->config, frameCallback(serialNumber)); my_dev->started = true; @@ -992,6 +1042,7 @@ Orbbec::Orbbec(vsdk::Dependencies deps, vsdk::ResourceConfig cfg, std::shared_pt throw std::runtime_error("Failed to detect Orbbec model configuration"); } + configureDevice(serial_number_, model_config_.value()); startDevice(serial_number_, model_config_.value()); { std::lock_guard lock(serial_by_resource_mu()); @@ -1081,6 +1132,7 @@ void Orbbec::reconfigure(const vsdk::Dependencies& deps, const vsdk::ResourceCon throw std::runtime_error("Failed to detect Orbbec model configuration during reconfigure"); } + configureDevice(new_serial_number, model_config_.value()); startDevice(new_serial_number, model_config_.value()); { std::lock_guard lock(serial_by_resource_mu()); @@ -1591,80 +1643,24 @@ std::unique_ptr Orbbec::configure(vsdk::Dependencies d // ORBBEC SDK DEVICE REGISTRY START void registerDevice(std::string serialNumber, std::shared_ptr dev) { - VIAM_SDK_LOG(info) << "starting " << serialNumber; - std::shared_ptr pipe = std::make_shared(dev); - pipe->enableFrameSync(); - - // Determine model configuration - std::shared_ptr deviceInfo = dev->getDeviceInfo(); - std::optional modelConfig = OrbbecModelConfig::forDevice(deviceInfo->name()); - if (!modelConfig.has_value()) { - VIAM_SDK_LOG(error) << "Failed to determine model configuration for device " << serialNumber; - return; - } - - std::shared_ptr config = createHwD2CAlignConfig(pipe, std::nullopt, std::nullopt, *modelConfig); - if (config == nullptr) { - VIAM_SDK_LOG(warn) << "Device " << serialNumber << " does not support hardware depth-to-color alignment, trying software alignment"; - - // Try to create a basic config without hardware alignment - try { - auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); - auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); - - if (colorStreamProfiles->getCount() > 0 && depthStreamProfiles->getCount() > 0) { - config = std::make_shared(); - config->enableStream(colorStreamProfiles->getProfile(0)); - config->enableStream(depthStreamProfiles->getProfile(0)); - config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); - VIAM_SDK_LOG(info) << "Created basic config for device " << serialNumber; - } else { - VIAM_SDK_LOG(error) << "Device " << serialNumber << " has no available stream profiles"; - return; - } - } catch (ob::Error& e) { - VIAM_SDK_LOG(error) << "Failed to create basic config for device " << serialNumber << ": " << e.what(); - return; - } - } + VIAM_SDK_LOG(info) << "[registerDevice] Registering device " << serialNumber; #ifdef _WIN32 // Setup Windows device registry for Orbbec cameras try { windows_registry::setupWindowsDeviceRegistry(dev); } catch (const std::exception& e) { - VIAM_SDK_LOG(error) << "Windows registry setup failed: " << e.what(); - // Continue anyway - this is not critical for functionality + throw std::runtime_error("failed to setup windows device registry: " + std::string(e.what())); } #endif - auto depthSensor = dev->getSensor(OB_SENSOR_DEPTH); - if (depthSensor == nullptr) { - throw std::runtime_error("Current device does not have a depth sensor."); - } - auto depthFilterList = depthSensor->createRecommendedFilters(); - - // NOTE: Swap this to depth if you want to align to depth - std::shared_ptr align = std::make_shared(OB_STREAM_COLOR); - - std::shared_ptr pointCloudFilter = std::make_shared(); - pointCloudFilter->setCreatePointFormat(OB_FORMAT_RGB_POINT); { std::lock_guard lock(devices_by_serial_mu()); std::unique_ptr my_dev = std::make_unique(); - - my_dev->pipe = pipe; my_dev->device = dev; - my_dev->serial_number = serialNumber; - my_dev->pointCloudFilter = pointCloudFilter; - my_dev->align = align; - my_dev->config = config; - my_dev->postProcessDepthFilters = - depthFilterList; // We save the recommended post process filters as the default, user can modify later via do_command - my_dev->applyEnabledPostProcessDepthFilters = - false; // We don't apply the post process filters by default, user can enable via `apply_post_process_depth_filters` do_command - + my_dev->serialNumber = serialNumber; devices_by_serial()[serialNumber] = std::move(my_dev); + VIAM_SDK_LOG(info) << "[registerDevice] Successfully registered device " << serialNumber; } } @@ -1707,6 +1703,7 @@ void deviceChangedCallback(const std::shared_ptr removedList, co VIAM_SDK_LOG(error) << "Failed to determine model configuration for device " << serial_number; continue; } + configureDevice(serial_number, modelConfig.value()); startDevice(serial_number, modelConfig.value()); serial_by_resource()[resource_name] = serial_number; } diff --git a/src/module/orbbec.hpp b/src/module/orbbec.hpp index de7e8994..1dff562e 100644 --- a/src/module/orbbec.hpp +++ b/src/module/orbbec.hpp @@ -118,9 +118,9 @@ struct OrbbecModelConfig { struct ViamOBDevice { ~ViamOBDevice() { - std::cout << "deleting ViamOBDevice " << serial_number << "\n"; + std::cout << "deleting ViamOBDevice " << serialNumber << "\n"; } - std::string serial_number{}; + std::string serialNumber{}; std::shared_ptr device{}; bool started{}; std::shared_ptr pipe{}; From 605bb3a16df8407c55dd303a9d8c5e3828034a6c Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Thu, 30 Oct 2025 12:31:22 -0400 Subject: [PATCH 31/33] [RSDK-12362] Fixing a bug to handle when return a nullptr, improving README per feedback, fixing some nits --- README.md | 10 +- src/module/orbbec.cpp | 269 +++++++++++++++++---------------- src/module/orbbec_firmware.cpp | 2 +- src/module/orbbec_firmware.hpp | 2 +- 4 files changed, 145 insertions(+), 138 deletions(-) diff --git a/README.md b/README.md index 6b5034e0..4423672f 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ This module provides access to the color and depth sensors and creates pointclou ## Model viam:orbbec:astra2 -Use [Orbbec cameras](https://www.orbbec.com/products/structured-light-camera/astra-2/). +[Official Orbbec Astra 2 Camera Webpage](https://www.orbbec.com/products/structured-light-camera/astra-2/). ### Configuration The following attribute template can be used to configure this model: @@ -35,7 +35,7 @@ The following attribute template can be used to configure this model: ``` #### Configuration Attributes -The following attributes are available for the Astra2 model: +The following attributes are available for the Astra 2 model: | Name | Type | Inclusion | Description | |---------------|--------|-----------|----------------------------| @@ -68,7 +68,7 @@ The following attributes are available for the Astra2 model: ## Model viam:orbbec:gemini_335le -Use [Orbbec cameras](https://www.orbbec.com/gemini-335le/). +[Official Orbbec Gemini 335Le Camera Webpage](https://www.orbbec.com/gemini-335le/). ### Configuration The following attribute template can be used to configure this model: @@ -92,7 +92,7 @@ The following attribute template can be used to configure this model: ``` #### Configuration Attributes -The following attributes are available for the Astra2 model: +The following attributes are available for the Gemini 335Le model: | Name | Type | Inclusion | Description | |---------------|--------|-----------|----------------------------| @@ -141,7 +141,7 @@ Bear in mind that the distortion parameters contained in that struct are not nam ## DoCommand You can use DoCommand to upgrade the firmware of your device to the required version. -Update the astra2 firmware to v2.8.20 and gemini_335le to v1.5.55. If running on macOS, you must manually unplug and replug the device after this returns. +Update the astra 2 firmware to v2.8.20 and gemini_335le to v1.5.55. If running on macOS, you must manually unplug and replug the device after this returns. **WARNING**: Do not unplug the device while the firmware update is in progress. { diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 3a4c2742..ec9bc60c 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -435,101 +435,124 @@ bool checkIfSupportHWD2CAlign(std::shared_ptr pipe, return false; } -// create a config for software depth-to-color alignment with specified resolution/format -std::shared_ptr createSwD2CAlignConfig(std::shared_ptr pipe, - std::optional deviceRes, - std::optional deviceFormat, - const OrbbecModelConfig& modelConfig) { - auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); - auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); - - if (colorStreamProfiles->getCount() == 0 || depthStreamProfiles->getCount() == 0) { - throw std::runtime_error("No stream profiles available"); - } - - auto config = std::make_shared(); - auto colorSpCount = colorStreamProfiles->getCount(); - auto depthSpCount = depthStreamProfiles->getCount(); - - std::shared_ptr colorProfile = nullptr; - std::shared_ptr depthProfile = nullptr; - - // Look for matching color profile - for (uint32_t i = 0; i < colorSpCount; i++) { - auto profile = colorStreamProfiles->getProfile(i); - auto vsp = profile->as(); - - bool resolutionMatch = true; - bool formatMatch = true; - - // Check resolution if specified - if (deviceRes.has_value() && deviceRes->color_resolution.has_value()) { - resolutionMatch = (vsp->getWidth() == deviceRes->color_resolution->width && - vsp->getHeight() == deviceRes->color_resolution->height); +// Helper function to check if a video stream profile matches the given resolution and format +bool profileMatchesSpec(std::shared_ptr vsp, + std::optional deviceRes, + std::optional deviceFormat, + const OrbbecModelConfig& modelConfig, + bool isColor) { + if (deviceFormat.has_value()) { + std::string requestedFormat = isColor ? + (deviceFormat->color_format.has_value() ? deviceFormat->color_format.value() : "") : + (deviceFormat->depth_format.has_value() ? deviceFormat->depth_format.value() : ""); + if (!requestedFormat.empty() && ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) != requestedFormat) { + return false; } - - // Check format if specified - if (deviceFormat.has_value() && deviceFormat->color_format.has_value()) { - formatMatch = (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == deviceFormat->color_format.value()); + } else { + std::string defaultFormat = isColor ? modelConfig.default_color_format : modelConfig.default_depth_format; + if (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) != defaultFormat) { + return false; } + } - if (resolutionMatch && formatMatch) { - colorProfile = profile; - break; + if (deviceRes.has_value()) { + auto requestedRes = isColor ? deviceRes->color_resolution : deviceRes->depth_resolution; + if (requestedRes.has_value() && + (vsp->getWidth() != requestedRes->width || vsp->getHeight() != requestedRes->height)) { + return false; + } + } else { + auto defaultRes = isColor ? modelConfig.default_color_resolution : modelConfig.default_depth_resolution; + if (vsp->getWidth() != defaultRes.width || vsp->getHeight() != defaultRes.height) { + return false; } } - // Look for matching depth profile - for (uint32_t i = 0; i < depthSpCount; i++) { - auto profile = depthStreamProfiles->getProfile(i); - auto vsp = profile->as(); - - bool resolutionMatch = true; - bool formatMatch = true; + return true; +} - // Check resolution if specified - if (deviceRes.has_value() && deviceRes->depth_resolution.has_value()) { - resolutionMatch = (vsp->getWidth() == deviceRes->depth_resolution->width && - vsp->getHeight() == deviceRes->depth_resolution->height); +// Helper function to find matching stream profiles +std::pair, std::shared_ptr> +findMatchingProfiles(std::shared_ptr pipe, + std::optional deviceRes, + std::optional deviceFormat, + const OrbbecModelConfig& modelConfig) { + auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); + auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); + + auto colorSpCount = colorStreamProfiles->getCount(); + auto depthSpCount = depthStreamProfiles->getCount(); + + for (uint32_t i = 0; i < colorSpCount; i++) { + auto colorProfile = colorStreamProfiles->getProfile(i); + auto colorVsp = colorProfile->as(); + + if (!profileMatchesSpec(colorVsp, deviceRes, deviceFormat, modelConfig, true)) { + continue; } - - // Check format if specified - if (deviceFormat.has_value() && deviceFormat->depth_format.has_value()) { - formatMatch = (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) == deviceFormat->depth_format.value()); + + for (uint32_t j = 0; j < depthSpCount; j++) { + auto depthProfile = depthStreamProfiles->getProfile(j); + auto depthVsp = depthProfile->as(); + + // make sure the color and depth stream have the same fps + if (colorVsp->getFps() != depthVsp->getFps()) { + continue; + } + + if (!profileMatchesSpec(depthVsp, deviceRes, deviceFormat, modelConfig, false)) { + continue; + } + + return std::make_pair(colorProfile, depthProfile); } + } + + return std::make_pair(nullptr, nullptr); +} - if (resolutionMatch && formatMatch) { - depthProfile = profile; - break; - } +// Helper function to build error message for missing profiles +std::string buildProfileErrorMsg(bool isColor, std::optional deviceRes, std::optional deviceFormat) { + std::ostringstream buffer; + buffer << service_name << " does not support the requested " << (isColor ? "color" : "depth") << " resolution/format: "; + auto res = isColor ? (deviceRes.has_value() ? deviceRes->color_resolution : std::nullopt) : + (deviceRes.has_value() ? deviceRes->depth_resolution : std::nullopt); + if (res.has_value()) { + buffer << "resolution " << res->width << "x" << res->height; } + auto fmt = isColor ? (deviceFormat.has_value() ? deviceFormat->color_format : std::nullopt) : + (deviceFormat.has_value() ? deviceFormat->depth_format : std::nullopt); + if (fmt.has_value()) { + buffer << ", format " << fmt.value(); + } + return buffer.str(); +} - // If no exact match found, throw an error since user specified requirements - if (!colorProfile) { - std::ostringstream buffer; - buffer << service_name << " does not support the requested color resolution/format: "; - if (deviceRes.has_value() && deviceRes->color_resolution.has_value()) { - buffer << "resolution " << deviceRes->color_resolution->width << "x" - << deviceRes->color_resolution->height; - } - if (deviceFormat.has_value() && deviceFormat->color_format.has_value()) { - buffer << ", format " << deviceFormat->color_format.value(); - } - throw std::runtime_error(buffer.str()); +// create a config for software depth-to-color alignment with specified resolution/format +std::shared_ptr createSwD2CAlignConfig(std::shared_ptr pipe, + std::optional deviceRes, + std::optional deviceFormat, + const OrbbecModelConfig& modelConfig) { + if (deviceRes.has_value()) { + VIAM_SDK_LOG(info) << "[createSwD2CAlignConfig] resolution specified: " << deviceRes->to_string(); } - if (!depthProfile) { - std::ostringstream buffer; - buffer << service_name << " does not support the requested depth resolution/format: "; - if (deviceRes.has_value() && deviceRes->depth_resolution.has_value()) { - buffer << "resolution " << deviceRes->depth_resolution->width << "x" - << deviceRes->depth_resolution->height; - } - if (deviceFormat.has_value() && deviceFormat->depth_format.has_value()) { - buffer << ", format " << deviceFormat->depth_format.value(); - } - throw std::runtime_error(buffer.str()); + if (deviceFormat.has_value()) { + VIAM_SDK_LOG(info) << "[createSwD2CAlignConfig] format specified: " << deviceFormat->to_string(); } + + // Find matching color and depth profiles + auto [colorProfile, depthProfile] = findMatchingProfiles(pipe, deviceRes, deviceFormat, modelConfig); + + if (!colorProfile || !depthProfile) { + VIAM_SDK_LOG(warn) << "[createSwD2CAlignConfig] Could not find matching stream profiles for software depth-to-color alignment that " + << "also match the given resolution and format specification (" + << (deviceRes.has_value() ? deviceRes->to_string() : "none") << ", " + << (deviceFormat.has_value() ? deviceFormat->to_string() : "none") << ")\n"; + return nullptr; + } + + auto config = std::make_shared(); config->enableStream(colorProfile); config->enableStream(depthProfile); config->setAlignMode(ALIGN_D2C_SW_MODE); // Use software alignment @@ -549,8 +572,6 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr std::optional deviceRes, std::optional deviceFormat, const OrbbecModelConfig& modelConfig) { - auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); - auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); if (deviceRes.has_value()) { VIAM_SDK_LOG(info) << "[createHwD2CAlignConfig] resolution specified: " << deviceRes->to_string(); } @@ -558,62 +579,35 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr VIAM_SDK_LOG(info) << "[createHwD2CAlignConfig] format specified: " << deviceFormat->to_string(); } - // Iterate through all color and depth stream profiles to find a match for - // hardware depth-to-color alignment + // Find matching color and depth profiles that support hardware D2C alignment + auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); + auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); + auto colorSpCount = colorStreamProfiles->getCount(); auto depthSpCount = depthStreamProfiles->getCount(); + for (uint32_t i = 0; i < colorSpCount; i++) { auto colorProfile = colorStreamProfiles->getProfile(i); auto colorVsp = colorProfile->as(); - - if (deviceFormat.has_value() && deviceFormat->color_format.has_value()) { - if (ob::TypeHelper::convertOBFormatTypeToString(colorVsp->getFormat()) != deviceFormat->color_format.value()) { - continue; - } - } else if (ob::TypeHelper::convertOBFormatTypeToString(colorVsp->getFormat()) != modelConfig.default_color_format) { - continue; - } - - if (deviceRes.has_value() && deviceRes->color_resolution.has_value()) { - if (colorVsp->getWidth() != deviceRes->color_resolution->width || - colorVsp->getHeight() != deviceRes->color_resolution->height) { - continue; - } - } else if (colorVsp->getWidth() != modelConfig.default_color_resolution.width || - colorVsp->getHeight() != modelConfig.default_color_resolution.height) { + + if (!profileMatchesSpec(colorVsp, deviceRes, deviceFormat, modelConfig, true)) { continue; } - + for (uint32_t j = 0; j < depthSpCount; j++) { auto depthProfile = depthStreamProfiles->getProfile(j); auto depthVsp = depthProfile->as(); - - // make sure the color and depth stream have the same fps, due to some - // models may not support different fps + + // make sure the color and depth stream have the same fps if (colorVsp->getFps() != depthVsp->getFps()) { continue; } - - if (deviceFormat.has_value() && deviceFormat->depth_format.has_value()) { - if (ob::TypeHelper::convertOBFormatTypeToString(depthVsp->getFormat()) != deviceFormat->depth_format.value()) { - continue; - } - } else if (ob::TypeHelper::convertOBFormatTypeToString(depthVsp->getFormat()) != modelConfig.default_depth_format) { - continue; - } - - if (deviceRes.has_value() && deviceRes->depth_resolution.has_value()) { - if (depthVsp->getWidth() != deviceRes->depth_resolution->width || - depthVsp->getHeight() != deviceRes->depth_resolution->height) { - continue; - } - } else if (depthVsp->getWidth() != modelConfig.default_depth_resolution.width || - depthVsp->getHeight() != modelConfig.default_depth_resolution.height) { + + if (!profileMatchesSpec(depthVsp, deviceRes, deviceFormat, modelConfig, false)) { continue; } - - // Check if the given stream profiles support hardware depth-to-color - // alignment + + // Check if the given stream profiles support hardware depth-to-color alignment if (checkIfSupportHWD2CAlign(pipe, colorProfile, depthProfile)) { VIAM_SDK_LOG(info) << "[createHwD2CAlignConfig] Using hardware depth-to-color alignment with color stream " << colorVsp->getWidth() << "x" << colorVsp->getHeight() << "@" << colorVsp->getFps() @@ -626,14 +620,10 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr hwD2CAlignConfig->enableStream(colorProfile); // enable color stream hwD2CAlignConfig->enableStream(depthProfile); // enable depth stream hwD2CAlignConfig->setAlignMode(ALIGN_D2C_HW_MODE); // enable hardware depth-to-color alignment - hwD2CAlignConfig->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); // output - // frameset - // with all - // types of - // frames + hwD2CAlignConfig->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); return hwD2CAlignConfig; } else { - VIAM_SDK_LOG(error) << "[createHwD2CAlignConfig] color stream " << colorVsp->getWidth() << "x" << colorVsp->getHeight() + VIAM_SDK_LOG(warn) << "[createHwD2CAlignConfig] color stream " << colorVsp->getWidth() << "x" << colorVsp->getHeight() << "@" << colorVsp->getFps() << " and depth stream " << depthVsp->getWidth() << "x" << depthVsp->getHeight() << "@" << depthVsp->getFps() << " do NOT support hardware depth-to-color alignment\n"; @@ -641,7 +631,7 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr } } - VIAM_SDK_LOG(error) << "[createHwD2CAlignConfig] Could not find matching stream profiles for hardware depth-to-color alignment that " + VIAM_SDK_LOG(warn) << "[createHwD2CAlignConfig] Could not find matching stream profiles for hardware depth-to-color alignment that " "also match the given resolution and format specification (" << (deviceRes.has_value() ? deviceRes->to_string() : "none") << ", " << (deviceFormat.has_value() ? deviceFormat->to_string() : "none") << ")\n"; @@ -752,6 +742,7 @@ void configureDevice(std::string serialNumber, OrbbecModelConfig const& modelCon } // Create the pipeline config (with user settings if provided, otherwise defaults) + // Lets try hardware depth-to-color alignment first, if it fails, try software alignment auto config = createHwD2CAlignConfig(my_dev->pipe, resolution_opt, format_opt, modelConfig); if (config == nullptr) { VIAM_SDK_LOG(warn) << "Device " << serialNumber @@ -759,8 +750,13 @@ void configureDevice(std::string serialNumber, OrbbecModelConfig const& modelCon // Use software alignment if (resolution_opt.has_value() || format_opt.has_value()) { config = createSwD2CAlignConfig(my_dev->pipe, resolution_opt, format_opt, modelConfig); + if (config == nullptr) { + std::ostringstream buffer; + buffer << service_name << ": unable to configure device " << serialNumber << " with software D2C alignment for the requested resolution/format"; + throw std::runtime_error(buffer.str()); + } } else { - // Create basic config with first available profiles + // If the user didn't specify a resolution or format, use the first available profiles auto colorStreamProfiles = my_dev->pipe->getStreamProfileList(OB_SENSOR_COLOR); auto depthStreamProfiles = my_dev->pipe->getStreamProfileList(OB_SENSOR_DEPTH); if (colorStreamProfiles->getCount() > 0 && depthStreamProfiles->getCount() > 0) { @@ -779,6 +775,12 @@ void configureDevice(std::string serialNumber, OrbbecModelConfig const& modelCon } else { VIAM_SDK_LOG(info) << "Device " << serialNumber << " supports hardware depth-to-color alignment, using it"; } + + if (config == nullptr) { + std::ostringstream buffer; + buffer << service_name << ": unable to configure device " << serialNumber << " - no valid stream configuration found"; + throw std::runtime_error(buffer.str()); + } my_dev->config = config; } @@ -793,6 +795,11 @@ void startDevice(std::string serialNumber, OrbbecModelConfig const& modelConfig) } std::shared_ptr deviceInfo = search->second->device->getDeviceInfo(); + if (deviceInfo == nullptr) { + std::ostringstream buffer; + buffer << service_name << ": unable to get device info for device " << serialNumber; + throw std::runtime_error(buffer.str()); + } std::string deviceName = deviceInfo->name(); VIAM_SDK_LOG(info) << "Device " << serialNumber << " is supported: " << deviceName; diff --git a/src/module/orbbec_firmware.cpp b/src/module/orbbec_firmware.cpp index bcfe19d3..f6510cf5 100644 --- a/src/module/orbbec_firmware.cpp +++ b/src/module/orbbec_firmware.cpp @@ -180,4 +180,4 @@ void updateFirmware(std::unique_ptr& my_dev, #endif } } // namespace firmware -} // namespace orbbec \ No newline at end of file +} // namespace orbbec diff --git a/src/module/orbbec_firmware.hpp b/src/module/orbbec_firmware.hpp index 97421c9a..b369434f 100644 --- a/src/module/orbbec_firmware.hpp +++ b/src/module/orbbec_firmware.hpp @@ -38,4 +38,4 @@ void updateFirmware(std::unique_ptr& my_dev, viam::sdk::LogSource& logger); } // namespace firmware -} // namespace orbbec \ No newline at end of file +} // namespace orbbec From 32087d0c7de8b596b22ed14f57773c8b14a8c20b Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Thu, 30 Oct 2025 12:42:51 -0400 Subject: [PATCH 32/33] [RSDK-12362] Making lint happy and adding the lint step to Makefile module.tar.gz rule --- Makefile | 2 +- src/module/orbbec.cpp | 101 ++++++++++++++++++++---------------------- 2 files changed, 49 insertions(+), 54 deletions(-) diff --git a/Makefile b/Makefile index a23f28ce..993d5b1a 100644 --- a/Makefile +++ b/Makefile @@ -76,7 +76,7 @@ else --build=missing endif -module.tar.gz: conan-pkg meta.json +module.tar.gz: lint conan-pkg meta.json ifeq ($(OS),Windows_NT) cmd /C "refreshenv && IF EXIST .\venv\Scripts\activate.bat call .\venv\Scripts\activate.bat && conan install --requires=viam-orbbec/0.0.1 -o:a "viam-cpp-sdk/*:shared=False" -s:a build_type=Release -s:a compiler.cppstd=17 --deployer-package "^&" --envs-generation false" else diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index ec9bc60c..76d4db09 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -437,14 +437,13 @@ bool checkIfSupportHWD2CAlign(std::shared_ptr pipe, // Helper function to check if a video stream profile matches the given resolution and format bool profileMatchesSpec(std::shared_ptr vsp, - std::optional deviceRes, - std::optional deviceFormat, - const OrbbecModelConfig& modelConfig, - bool isColor) { + std::optional deviceRes, + std::optional deviceFormat, + const OrbbecModelConfig& modelConfig, + bool isColor) { if (deviceFormat.has_value()) { - std::string requestedFormat = isColor ? - (deviceFormat->color_format.has_value() ? deviceFormat->color_format.value() : "") : - (deviceFormat->depth_format.has_value() ? deviceFormat->depth_format.value() : ""); + std::string requestedFormat = isColor ? (deviceFormat->color_format.has_value() ? deviceFormat->color_format.value() : "") + : (deviceFormat->depth_format.has_value() ? deviceFormat->depth_format.value() : ""); if (!requestedFormat.empty() && ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) != requestedFormat) { return false; } @@ -457,8 +456,7 @@ bool profileMatchesSpec(std::shared_ptr vsp, if (deviceRes.has_value()) { auto requestedRes = isColor ? deviceRes->color_resolution : deviceRes->depth_resolution; - if (requestedRes.has_value() && - (vsp->getWidth() != requestedRes->width || vsp->getHeight() != requestedRes->height)) { + if (requestedRes.has_value() && (vsp->getWidth() != requestedRes->width || vsp->getHeight() != requestedRes->height)) { return false; } } else { @@ -472,42 +470,42 @@ bool profileMatchesSpec(std::shared_ptr vsp, } // Helper function to find matching stream profiles -std::pair, std::shared_ptr> -findMatchingProfiles(std::shared_ptr pipe, - std::optional deviceRes, - std::optional deviceFormat, - const OrbbecModelConfig& modelConfig) { +std::pair, std::shared_ptr> findMatchingProfiles( + std::shared_ptr pipe, + std::optional deviceRes, + std::optional deviceFormat, + const OrbbecModelConfig& modelConfig) { auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); - + auto colorSpCount = colorStreamProfiles->getCount(); auto depthSpCount = depthStreamProfiles->getCount(); - + for (uint32_t i = 0; i < colorSpCount; i++) { auto colorProfile = colorStreamProfiles->getProfile(i); auto colorVsp = colorProfile->as(); - + if (!profileMatchesSpec(colorVsp, deviceRes, deviceFormat, modelConfig, true)) { continue; } - + for (uint32_t j = 0; j < depthSpCount; j++) { auto depthProfile = depthStreamProfiles->getProfile(j); auto depthVsp = depthProfile->as(); - + // make sure the color and depth stream have the same fps if (colorVsp->getFps() != depthVsp->getFps()) { continue; } - + if (!profileMatchesSpec(depthVsp, deviceRes, deviceFormat, modelConfig, false)) { continue; } - + return std::make_pair(colorProfile, depthProfile); } } - + return std::make_pair(nullptr, nullptr); } @@ -515,13 +513,13 @@ findMatchingProfiles(std::shared_ptr pipe, std::string buildProfileErrorMsg(bool isColor, std::optional deviceRes, std::optional deviceFormat) { std::ostringstream buffer; buffer << service_name << " does not support the requested " << (isColor ? "color" : "depth") << " resolution/format: "; - auto res = isColor ? (deviceRes.has_value() ? deviceRes->color_resolution : std::nullopt) : - (deviceRes.has_value() ? deviceRes->depth_resolution : std::nullopt); + auto res = isColor ? (deviceRes.has_value() ? deviceRes->color_resolution : std::nullopt) + : (deviceRes.has_value() ? deviceRes->depth_resolution : std::nullopt); if (res.has_value()) { buffer << "resolution " << res->width << "x" << res->height; } - auto fmt = isColor ? (deviceFormat.has_value() ? deviceFormat->color_format : std::nullopt) : - (deviceFormat.has_value() ? deviceFormat->depth_format : std::nullopt); + auto fmt = isColor ? (deviceFormat.has_value() ? deviceFormat->color_format : std::nullopt) + : (deviceFormat.has_value() ? deviceFormat->depth_format : std::nullopt); if (fmt.has_value()) { buffer << ", format " << fmt.value(); } @@ -540,10 +538,9 @@ std::shared_ptr createSwD2CAlignConfig(std::shared_ptr VIAM_SDK_LOG(info) << "[createSwD2CAlignConfig] format specified: " << deviceFormat->to_string(); } - // Find matching color and depth profiles auto [colorProfile, depthProfile] = findMatchingProfiles(pipe, deviceRes, deviceFormat, modelConfig); - + if (!colorProfile || !depthProfile) { VIAM_SDK_LOG(warn) << "[createSwD2CAlignConfig] Could not find matching stream profiles for software depth-to-color alignment that " << "also match the given resolution and format specification (" @@ -560,9 +557,8 @@ std::shared_ptr createSwD2CAlignConfig(std::shared_ptr auto colorVsp = colorProfile->as(); auto depthVsp = depthProfile->as(); - VIAM_SDK_LOG(info) << "Using software depth-to-color alignment with color " - << colorVsp->getWidth() << "x" << colorVsp->getHeight() << " and depth " << depthVsp->getWidth() - << "x" << depthVsp->getHeight(); + VIAM_SDK_LOG(info) << "Using software depth-to-color alignment with color " << colorVsp->getWidth() << "x" << colorVsp->getHeight() + << " and depth " << depthVsp->getWidth() << "x" << depthVsp->getHeight(); return config; } @@ -582,31 +578,31 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr // Find matching color and depth profiles that support hardware D2C alignment auto colorStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_COLOR); auto depthStreamProfiles = pipe->getStreamProfileList(OB_SENSOR_DEPTH); - + auto colorSpCount = colorStreamProfiles->getCount(); auto depthSpCount = depthStreamProfiles->getCount(); - + for (uint32_t i = 0; i < colorSpCount; i++) { auto colorProfile = colorStreamProfiles->getProfile(i); auto colorVsp = colorProfile->as(); - + if (!profileMatchesSpec(colorVsp, deviceRes, deviceFormat, modelConfig, true)) { continue; } - + for (uint32_t j = 0; j < depthSpCount; j++) { auto depthProfile = depthStreamProfiles->getProfile(j); auto depthVsp = depthProfile->as(); - + // make sure the color and depth stream have the same fps if (colorVsp->getFps() != depthVsp->getFps()) { continue; } - + if (!profileMatchesSpec(depthVsp, deviceRes, deviceFormat, modelConfig, false)) { continue; } - + // Check if the given stream profiles support hardware depth-to-color alignment if (checkIfSupportHWD2CAlign(pipe, colorProfile, depthProfile)) { VIAM_SDK_LOG(info) << "[createHwD2CAlignConfig] Using hardware depth-to-color alignment with color stream " @@ -624,17 +620,17 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr return hwD2CAlignConfig; } else { VIAM_SDK_LOG(warn) << "[createHwD2CAlignConfig] color stream " << colorVsp->getWidth() << "x" << colorVsp->getHeight() - << "@" << colorVsp->getFps() << " and depth stream " << depthVsp->getWidth() << "x" - << depthVsp->getHeight() << "@" << depthVsp->getFps() - << " do NOT support hardware depth-to-color alignment\n"; + << "@" << colorVsp->getFps() << " and depth stream " << depthVsp->getWidth() << "x" + << depthVsp->getHeight() << "@" << depthVsp->getFps() + << " do NOT support hardware depth-to-color alignment\n"; } } } VIAM_SDK_LOG(warn) << "[createHwD2CAlignConfig] Could not find matching stream profiles for hardware depth-to-color alignment that " - "also match the given resolution and format specification (" - << (deviceRes.has_value() ? deviceRes->to_string() : "none") << ", " - << (deviceFormat.has_value() ? deviceFormat->to_string() : "none") << ")\n"; + "also match the given resolution and format specification (" + << (deviceRes.has_value() ? deviceRes->to_string() : "none") << ", " + << (deviceFormat.has_value() ? deviceFormat->to_string() : "none") << ")\n"; return nullptr; } @@ -703,7 +699,7 @@ void configureDevice(std::string serialNumber, OrbbecModelConfig const& modelCon } std::unique_ptr& my_dev = search->second; - + // Initialize fields if not already set if (!my_dev->pipe) { my_dev->pipe = std::make_shared(my_dev->device); @@ -724,7 +720,7 @@ void configureDevice(std::string serialNumber, OrbbecModelConfig const& modelCon my_dev->postProcessDepthFilters = depthSensor->createRecommendedFilters(); my_dev->applyEnabledPostProcessDepthFilters = false; } - + // Get user-specified resolution/format if any std::optional resolution_opt; std::optional format_opt; @@ -745,14 +741,14 @@ void configureDevice(std::string serialNumber, OrbbecModelConfig const& modelCon // Lets try hardware depth-to-color alignment first, if it fails, try software alignment auto config = createHwD2CAlignConfig(my_dev->pipe, resolution_opt, format_opt, modelConfig); if (config == nullptr) { - VIAM_SDK_LOG(warn) << "Device " << serialNumber - << " does not support hardware depth-to-color alignment, trying software alignment"; + VIAM_SDK_LOG(warn) << "Device " << serialNumber << " does not support hardware depth-to-color alignment, trying software alignment"; // Use software alignment if (resolution_opt.has_value() || format_opt.has_value()) { config = createSwD2CAlignConfig(my_dev->pipe, resolution_opt, format_opt, modelConfig); if (config == nullptr) { std::ostringstream buffer; - buffer << service_name << ": unable to configure device " << serialNumber << " with software D2C alignment for the requested resolution/format"; + buffer << service_name << ": unable to configure device " << serialNumber + << " with software D2C alignment for the requested resolution/format"; throw std::runtime_error(buffer.str()); } } else { @@ -769,13 +765,13 @@ void configureDevice(std::string serialNumber, OrbbecModelConfig const& modelCon throw std::runtime_error("No stream profiles available"); } } - if(config != nullptr) { + if (config != nullptr) { VIAM_SDK_LOG(info) << "Device " << serialNumber << " supports software depth-to-color alignment, using it"; } } else { VIAM_SDK_LOG(info) << "Device " << serialNumber << " supports hardware depth-to-color alignment, using it"; } - + if (config == nullptr) { std::ostringstream buffer; buffer << service_name << ": unable to configure device " << serialNumber << " - no valid stream configuration found"; @@ -810,7 +806,7 @@ void startDevice(std::string serialNumber, OrbbecModelConfig const& modelConfig) buffer << service_name << ": device " << serialNumber << " is already started"; throw std::runtime_error(buffer.str()); } - + // Start the pipeline with the configuration my_dev->pipe->start(my_dev->config, frameCallback(serialNumber)); my_dev->started = true; @@ -964,7 +960,6 @@ std::vector Orbbec::validateOrbbecModel(vsdk::ResourceConfig cfg, O return {}; } - std::vector Orbbec::validateAstra2(vsdk::ResourceConfig cfg) { return validateOrbbecModel(cfg, ASTRA2_CONFIG); } From 0bf051bbb9986487c5d999c0dcdfba3083de2793 Mon Sep 17 00:00:00 2001 From: Sebastian Munoz Date: Thu, 30 Oct 2025 14:09:24 -0400 Subject: [PATCH 33/33] [RSDK-12362] Disabling Gemini cameras for hosts other than Linux for the moment --- src/module/orbbec.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 76d4db09..06086be5 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -965,7 +965,11 @@ std::vector Orbbec::validateAstra2(vsdk::ResourceConfig cfg) { } std::vector Orbbec::validateGemini335Le(vsdk::ResourceConfig cfg) { +#if !defined(__linux__) + return {"viam:orbbec:gemini_335le is only supported on Linux hosts"}; +#else return validateOrbbecModel(cfg, GEMINI_335LE_CONFIG); +#endif } std::string getSerialNumber(const vsdk::ResourceConfig& cfg) {