diff --git a/CMakeLists.txt b/CMakeLists.txt index 8d7ab74..7075c69 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/Makefile b/Makefile index a23f28c..993d5b1 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/README.md b/README.md index c190f71..4423672 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 this model: +The following attributes are available for the Astra 2 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 +[Official Orbbec Gemini 335Le Camera Webpage](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 Gemini 335Le 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 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/device_control.hpp b/src/module/device_control.hpp index 6ff05cd..9be6465 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; @@ -630,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/discovery.cpp b/src/module/discovery.cpp index 8576ae8..ad39459 100644 --- a/src/module/discovery.cpp +++ b/src/module/discovery.cpp @@ -12,35 +12,91 @@ 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(error) << "Failed to enable network device enumeration: " << e.what(); + } catch (const std::exception& e) { + VIAM_RESOURCE_LOG(error) << "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; + } - std::ostringstream name; - name << "orbbec-" << i + 1; + VIAM_RESOURCE_LOG(info) << deviceInfoString.str(); - vsdk::ProtoStruct attributes; - attributes.emplace("serial_number", info->serialNumber()); + std::ostringstream name; + name << "orbbec-" << i + 1; - 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::ProtoStruct attributes; + attributes.emplace("serial_number", serialNumber); + + // Detect model and create appropriate resource + std::string viamModelSuffix; + 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()), + "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(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(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(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(error) << "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/main.cpp b/src/module/main.cpp index e386a83..2800130 100644 --- a/src/module/main.cpp +++ b/src/module/main.cpp @@ -17,17 +17,25 @@ 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) { 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 f357788..06086be 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,8 @@ namespace orbbec { namespace vsdk = ::viam::sdk; -vsdk::Model Orbbec::model("viam", "orbbec", "astra2"); -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}}}}; +vsdk::Model Orbbec::model_astra2("viam", "orbbec", "astra2"); +vsdk::Model Orbbec::model_gemini_335le("viam", "orbbec", "gemini_335le"); // CONSTANTS BEGIN const std::string kColorSourceName = "color"; @@ -77,12 +59,67 @@ 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 { +static const OrbbecModelConfig ASTRA2_CONFIG{ + {"Orbbec Astra 2", "Orbbec Astra2"}, // model_names + "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 +}; + +static const OrbbecModelConfig GEMINI_335LE_CONFIG{ + {"Orbbec Gemini 335Le"}, // model_names + "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 +}; + +static const std::vector all_model_configs = {ASTRA2_CONFIG, GEMINI_335LE_CONFIG}; +} // namespace + +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 + 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; +} + // CONSTANTS END // STRUCTS BEGIN @@ -141,26 +178,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 + - ". 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."); } } -// 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(); } @@ -172,6 +203,118 @@ uint64_t timeSinceFrameUs(uint64_t nowUs, uint64_t imageTimeUs) { return 0; } +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, + 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) { + std::ostringstream buffer; + buffer << "unsupported color format: " << frameFormat << ", supported: "; + for (const auto& fmt : modelConfig.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 != modelConfig.default_color_format) { + 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) { + 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) { + std::ostringstream buffer; + buffer << "unsupported depth format: " << frameFormat << ", supported: "; + for (const auto& fmt : modelConfig.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 != modelConfig.default_depth_format) { + throw std::runtime_error(formatError("depth format mismatch, expected ", modelConfig.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); @@ -292,42 +435,57 @@ bool checkIfSupportHWD2CAlign(std::shared_ptr pipe, return false; } -// create a config for hardware depth-to-color alignment -std::shared_ptr createHwD2CAlignConfig(std::shared_ptr pipe, - std::optional deviceRes, - std::optional deviceFormat) { - 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(); - } +// 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()) { - VIAM_SDK_LOG(info) << "[createHwD2CAlignConfig] format specified: " << deviceFormat->to_string(); + 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; + } + } else { + std::string defaultFormat = isColor ? modelConfig.default_color_format : modelConfig.default_depth_format; + if (ob::TypeHelper::convertOBFormatTypeToString(vsp->getFormat()) != defaultFormat) { + return false; + } + } + + 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; + } } - // Iterate through all color and depth stream profiles to find a match for - // hardware depth-to-color alignment + return true; +} + +// 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 (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()) != Orbbec::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() != Orbbec::default_color_resolution.width || - colorVsp->getHeight() != Orbbec::default_color_resolution.height) { + if (!profileMatchesSpec(colorVsp, deviceRes, deviceFormat, modelConfig, true)) { continue; } @@ -335,214 +493,145 @@ std::shared_ptr createHwD2CAlignConfig(std::shared_ptr 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()) != Orbbec::default_depth_format) { + if (!profileMatchesSpec(depthVsp, deviceRes, deviceFormat, modelConfig, false)) { 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() != Orbbec::default_depth_resolution.width || - depthVsp->getHeight() != Orbbec::default_depth_resolution.height) { - 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 " - << colorVsp->getWidth() << "x" << colorVsp->getHeight() << "@" << colorVsp->getFps() - << ", format: " << ob::TypeHelper::convertOBFormatTypeToString(colorVsp->getFormat()) - << " and depth stream " << depthVsp->getWidth() << "x" << depthVsp->getHeight() << "@" - << depthVsp->getFps() - << " format: " << ob::TypeHelper::convertOBFormatTypeToString(depthVsp->getFormat()) << "\n"; - // If support, create a config for hardware depth-to-color alignment - auto hwD2CAlignConfig = std::make_shared(); - 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 - return hwD2CAlignConfig; - } else { - VIAM_SDK_LOG(error) << "[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"; - } + return std::make_pair(colorProfile, depthProfile); } } - VIAM_SDK_LOG(error) << "[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"; - return nullptr; + return std::make_pair(nullptr, 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; +// 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(); } -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"); +// 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(); } - - // 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()); + if (deviceFormat.has_value()) { + VIAM_SDK_LOG(info) << "[createSwD2CAlignConfig] format specified: " << deviceFormat->to_string(); } - std::vector binData; - zip_error_t ziperror; - zip_error_init(&ziperror); + // Find matching color and depth profiles + auto [colorProfile, depthProfile] = findMatchingProfiles(pipe, deviceRes, deviceFormat, modelConfig); - 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()); + 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; } - // Ensure src cleanup if zip_open fails - CleanupPtr srcCleanup(src); + auto config = std::make_shared(); + 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); - // 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()); - } + 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(); - srcCleanup.release(); - CleanupPtr zipCleanup(zip); + return config; +} - if (zip_get_num_entries(zip, 0) != 1) { - throw std::runtime_error("unexpected number of files in firmware zip"); +// create a config for hardware depth-to-color alignment +std::shared_ptr createHwD2CAlignConfig(std::shared_ptr pipe, + std::optional deviceRes, + std::optional deviceFormat, + const OrbbecModelConfig& modelConfig) { + if (deviceRes.has_value()) { + VIAM_SDK_LOG(info) << "[createHwD2CAlignConfig] resolution specified: " << deviceRes->to_string(); } - - const char* fileName = zip_get_name(zip, 0, 0); - if (!fileName) { - throw std::runtime_error("couldn't get bin file name"); + if (deviceFormat.has_value()) { + VIAM_SDK_LOG(info) << "[createHwD2CAlignConfig] format specified: " << deviceFormat->to_string(); } - CleanupPtr binFile(zip_fopen(zip, fileName, 0)); - if (!binFile) { - throw std::runtime_error("failed to open the firmware bin file"); - } + // 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); - zip_stat_t stats; - zip_stat_init(&stats); - if (zip_stat(zip, fileName, 0, &stats) != 0) { - throw std::invalid_argument("failed to stat file"); - } + auto colorSpCount = colorStreamProfiles->getCount(); + auto depthSpCount = depthStreamProfiles->getCount(); - 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()); - } + for (uint32_t i = 0; i < colorSpCount; i++) { + auto colorProfile = colorStreamProfiles->getProfile(i); + auto colorVsp = colorProfile->as(); - 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()); - } + if (!profileMatchesSpec(colorVsp, deviceRes, deviceFormat, modelConfig, true)) { + continue; + } - 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"; - }; + for (uint32_t j = 0; j < depthSpCount; j++) { + auto depthProfile = depthStreamProfiles->getProfile(j); + auto depthVsp = depthProfile->as(); - 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; + // 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 " + << colorVsp->getWidth() << "x" << colorVsp->getHeight() << "@" << colorVsp->getFps() + << ", format: " << ob::TypeHelper::convertOBFormatTypeToString(colorVsp->getFormat()) + << " and depth stream " << depthVsp->getWidth() << "x" << depthVsp->getHeight() << "@" + << depthVsp->getFps() + << " format: " << ob::TypeHelper::convertOBFormatTypeToString(depthVsp->getFormat()) << "\n"; + // If support, create a config for hardware depth-to-color alignment + auto hwD2CAlignConfig = std::make_shared(); + 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); + 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"; + } + } } - // Reset UVC backend type on success -#if defined(__linux__) - ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_AUTO); -#endif + 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"; + return nullptr; } auto frameCallback(const std::string& serialNumber) { @@ -599,86 +688,129 @@ auto frameCallback(const std::string& serialNumber) { }; } -void startDevice(std::string serialNumber) { - VIAM_SDK_LOG(info) << service_name << ": starting device " << 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 start undetected device" << serialNumber; - throw std::invalid_argument(buffer.str()); - } - - // Check if the device is an Astra 2 - std::shared_ptr deviceInfo = search->second->device->getDeviceInfo(); - if (!strstr(deviceInfo->name(), "Astra 2")) { - std::ostringstream buffer; - buffer << service_name << ": device " << serialNumber << " is not an Astra 2 (found: " << deviceInfo->name() << ")"; + buffer << service_name << ": unable to configure undetected device " << serialNumber; throw std::invalid_argument(buffer.str()); } 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()); + + // Initialize fields if not already set + if (!my_dev->pipe) { + my_dev->pipe = std::make_shared(my_dev->device); + my_dev->pipe->enableFrameSync(); } - VIAM_SDK_LOG(info) << "[startDevice] Configuring device resolution"; + 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 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()); + 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"; } - 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"); + } + + // 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 << " does not support hardware depth-to-color alignment, trying software alignment"; + // Use software alignment if (resolution_opt.has_value() || format_opt.has_value()) { - // Create the pipeline - auto config = createHwD2CAlignConfig(search->second->pipe, resolution_opt, format_opt); + config = createSwD2CAlignConfig(my_dev->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(); + buffer << service_name << ": unable to configure device " << serialNumber + << " with software D2C alignment for the requested resolution/format"; throw std::runtime_error(buffer.str()); } - my_dev->config = config; + } else { + // 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) { + 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->pipe->start(my_dev->config, frameCallback(serialNumber)); - my_dev->started = true; + 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; +} - // 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)); +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); + if (search == devices_by_serial().end()) { + std::ostringstream buffer; + buffer << service_name << ": unable to start undetected device" << serialNumber; + throw std::invalid_argument(buffer.str()); + } + + 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; - 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; + std::unique_ptr& my_dev = search->second; + if (my_dev->started) { + std::ostringstream buffer; + buffer << service_name << ": device " << serialNumber << " is already started"; + throw std::runtime_error(buffer.str()); } - VIAM_SDK_LOG(info) << service_name << ": device started " << serialNumber; + // Start the pipeline with the configuration + my_dev->pipe->start(my_dev->config, frameCallback(serialNumber)); + my_dev->started = true; + VIAM_SDK_LOG(info) << "Started pipeline for device " << serialNumber; } void stopDevice(std::string serialNumber, std::string resourceName) { @@ -712,7 +844,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) { @@ -743,10 +875,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(); @@ -754,10 +886,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(); @@ -768,7 +900,7 @@ void Orbbec::validate_sensor(std::pair const } } -std::vector Orbbec::validate(vsdk::ResourceConfig cfg) { +std::vector Orbbec::validateOrbbecModel(vsdk::ResourceConfig cfg, OrbbecModelConfig const& modelConfig) { auto attrs = cfg.attributes(); if (!attrs.count("serial_number")) { @@ -792,17 +924,17 @@ std::vector Orbbec::validate(vsdk::ResourceConfig cfg) { } if (sensors) { for (auto const& sensor_pair : *sensors) { - validate_sensor(sensor_pair); + 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 (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 : 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(); @@ -812,12 +944,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 (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 : 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(); @@ -828,6 +960,18 @@ std::vector Orbbec::validate(vsdk::ResourceConfig cfg) { return {}; } +std::vector Orbbec::validateAstra2(vsdk::ResourceConfig cfg) { + return validateOrbbecModel(cfg, ASTRA2_CONFIG); +} + +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) { auto attrs = cfg.attributes(); if (attrs.count("serial_number")) { @@ -867,12 +1011,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(); } { @@ -887,44 +1031,53 @@ Orbbec::Orbbec(vsdk::Dependencies deps, vsdk::ResourceConfig cfg, std::shared_pt applyExperimentalConfig(my_dev, cfg.attributes()); } - startDevice(serial_number_); - { - 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_->viam_model_suffix; } } - VIAM_SDK_LOG(info) << "Orbbec constructor end " << serial_number_; + if (!model_config_.has_value()) { + 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()); + serial_by_resource()[config->resource_name] = 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_ - << " is not in config_by_serial, skipping erase"; + 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; prev_resource_name = config_by_serial().at(serial_number_).resource_name; } } 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 +1086,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 +1109,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(); } { @@ -964,12 +1117,6 @@ void Orbbec::reconfigure(const vsdk::Dependencies& deps, const vsdk::ResourceCon frame_set_by_serial().erase(prev_serial_number); } - startDevice(new_serial_number); - { - 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_); @@ -977,23 +1124,41 @@ 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()); } } - VIAM_SDK_LOG(info) << "[reconfigure] Orbbec reconfigure end"; + + if (!model_config_.has_value()) { + 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()); + serial_by_resource()[new_resource_name] = new_serial_number; + } + 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_); serial_number = serial_number_; } - checkFirmwareVersion(firmware_version_); + if (model_config_.has_value()) { + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->viam_model_suffix); + } std::shared_ptr fs = nullptr; { @@ -1005,20 +1170,6 @@ 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_SDK_LOG(error) << buffer.str(); - throw std::invalid_argument(buffer.str()); - } std::optional res_format_opt; { @@ -1029,67 +1180,19 @@ 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_SDK_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(); - 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_SDK_LOG(error) << buffer.str(); - throw std::invalid_argument(buffer.str()); - } - VIAM_SDK_LOG(debug) << "[get_image] end"; + validateColorFrame(color, res_format_opt, *model_config_); + vsdk::Camera::raw_image response = encodeColorFrame(color); + 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; { @@ -1130,30 +1233,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, @@ -1165,24 +1246,26 @@ 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_); serial_number = serial_number_; } - checkFirmwareVersion(firmware_version_); + if (model_config_.has_value()) { + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->viam_model_suffix); + } std::shared_ptr fs = nullptr; { @@ -1229,109 +1312,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_SDK_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()); - } - } + validateColorFrame(color, device_format_opt, *model_config_); - 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_SDK_LOG(error) << buffer.str(); - throw std::runtime_error(buffer.str()); - } + 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_SDK_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, *model_config_); unsigned char* depthData = (unsigned char*)depth->getData(); if (depthData == nullptr) { @@ -1347,7 +1336,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,9 +1346,9 @@ 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 " - "older of the two" - << "color timestamp was " << colorTS << " depth timestamp was " << depthTS; + VIAM_RESOURCE_LOG(info) << "color and depth timestamps differ, defaulting to " + "older of the two" + << "color timestamp was " << colorTS << " depth timestamp was " << depthTS; } // use the older of the two timestamps timestamp = (colorTS > depthTS) ? depthTS : colorTS; @@ -1371,10 +1360,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())); } } @@ -1399,26 +1388,36 @@ 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 (firmware_version_.find(minFirmwareVer) != std::string::npos) { + 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 " << minFirmwareVer; + 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_SDK_LOG(info) << "Updating device firmware..."; try { - updateFirmware(dev, ob_ctx_); - firmware_version_ = minFirmwareVer; + 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"); + } + firmware_version_ = model_config_->min_firmware_version; } 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 +1432,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 +1475,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,25 +1494,27 @@ 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_); serial_number = serial_number_; } - checkFirmwareVersion(firmware_version_); + if (model_config_.has_value()) { + checkFirmwareVersion(firmware_version_, model_config_->min_firmware_version, model_config_->viam_model_suffix); + } std::shared_ptr fs = nullptr; { @@ -1526,29 +1527,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, *model_config_); + validateDepthFrame(depth, std::nullopt, *model_config_); std::uint8_t* colorData = (std::uint8_t*)color->getData(); if (colorData == nullptr) { @@ -1600,12 +1582,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 - << " but is not a valid directory, using current working directory to store PCD file"; + 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 +1597,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())); } } @@ -1667,140 +1649,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(); - std::shared_ptr config = createHwD2CAlignConfig(pipe, std::nullopt, std::nullopt); - if (config == nullptr) { - VIAM_SDK_LOG(error) << "Current device does not support hardware depth-to-color " - "alignment."; - return; - } + VIAM_SDK_LOG(info) << "[registerDevice] Registering device " << serialNumber; #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())); + 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; } } @@ -1837,32 +1703,97 @@ 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 + 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; + } + configureDevice(serial_number, modelConfig.value()); + startDevice(serial_number, modelConfig.value()); serial_by_resource()[resource_name] = serial_number; } } } } } catch (ob::Error& e) { - std::cerr << "setDeviceChangedCallback\n" - << "function:" << e.getFunction() << "\nargs:" << e.getArgs() << "\nname:" << e.getName() << "\nmessage:" << e.what() - << "\ntype:" << e.getExceptionType() << std::endl; + VIAM_SDK_LOG(error) << "setDeviceChangedCallback\n" + << "function:" << e.getFunction() << "\nargs:" << e.getArgs() << "\nname:" << e.getName() + << "\nmessage:" << e.what() << "\ntype:" << e.getExceptionType() << std::endl; } } 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; + } + + 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()) { + 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"; + } + } catch (ob::Error& deviceError) { + VIAM_SDK_LOG(warn) << "Failed to process device " << (i + 1) << ": " << deviceError.what(); + // Continue with other devices even if one fails + } } - std::shared_ptr dev = devList->getDevice(i); - std::shared_ptr info = dev->getDeviceInfo(); - printDeviceInfo(info); - registerDevice(info->getSerialNumber(), dev); + } 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 diff --git a/src/module/orbbec.hpp b/src/module/orbbec.hpp index ab1f5c8..1dff562 100644 --- a/src/module/orbbec.hpp +++ b/src/module/orbbec.hpp @@ -96,11 +96,31 @@ struct ObResourceConfig { } }; +struct OrbbecModelConfig { + 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; + std::string min_firmware_version; + 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 std::optional forDevice(const std::string& device_name); + + // Check if a device name is supported + static bool isSupported(const std::string& device_name); +}; + 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{}; @@ -126,25 +146,27 @@ 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 std::vector validateOrbbecModel(viam::sdk::ResourceConfig cfg, OrbbecModelConfig const& modelConfig); static viam::sdk::GeometryConfig geometry; - static viam::sdk::Model model; - 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; + static viam::sdk::Model model_astra2; + static viam::sdk::Model model_gemini_335le; private: std::shared_ptr ob_ctx_; std::string firmware_version_; std::mutex serial_number_mu_; std::string serial_number_; + 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); + 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 diff --git a/src/module/orbbec_firmware.cpp b/src/module/orbbec_firmware.cpp new file mode 100644 index 0000000..f6510cf --- /dev/null +++ b/src/module/orbbec_firmware.cpp @@ -0,0 +1,183 @@ +// 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 { + +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"); + } + + 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__) + ctx->setUvcBackendType(OB_UVC_BACKEND_TYPE_AUTO); +#endif +} +} // namespace firmware +} // namespace orbbec diff --git a/src/module/orbbec_firmware.hpp b/src/module/orbbec_firmware.hpp new file mode 100644 index 0000000..b369434 --- /dev/null +++ b/src/module/orbbec_firmware.hpp @@ -0,0 +1,41 @@ +// 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 + +#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, + viam::sdk::LogSource& logger); + +} // namespace firmware +} // namespace orbbec diff --git a/src/module/orbbec_windows_registry.cpp b/src/module/orbbec_windows_registry.cpp new file mode 100644 index 0000000..004ebea --- /dev/null +++ b/src/module/orbbec_windows_registry.cpp @@ -0,0 +1,137 @@ +// 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 + +#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 0000000..b182ed3 --- /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