From 5ea21060a7f7cb98f268938cd4def1eb6969287b Mon Sep 17 00:00:00 2001 From: jj <957713278@qq.com> Date: Mon, 19 May 2025 10:40:35 +0800 Subject: [PATCH] Update net device connection --- .../orbbec_camera/ob_camera_node_driver.h | 10 + .../launch/gemini_330_series.launch.py | 6 + orbbec_camera/src/ob_camera_node_driver.cpp | 241 +++++++++++++++++- 3 files changed, 250 insertions(+), 7 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h index bb99ed49..623bba76 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h @@ -48,6 +48,9 @@ class OBCameraNodeDriver : public rclcpp::Node { std::shared_ptr selectDeviceByUSBPort(const std::shared_ptr& list, const std::string& usb_port); + std::shared_ptr selectDeviceByNetIP(const std::shared_ptr& list, + const std::string& net_ip); + void initializeDevice(const std::shared_ptr& device); void startDevice(const std::shared_ptr& list); @@ -68,6 +71,11 @@ class OBCameraNodeDriver : public rclcpp::Node { void rebootDeviceCallback(const std::shared_ptr request, std::shared_ptr response); + void presetUpdateCallback(bool firstCall, OBFwUpdateState state, const char* message, + uint8_t percent); + void updatePresetFirmware(std::string path); + + void firmwareUpdateCallback(OBFwUpdateState state, const char* message, uint8_t percent); private: const rclcpp::NodeOptions node_options_; @@ -104,9 +112,11 @@ class OBCameraNodeDriver : public rclcpp::Node { int net_device_port_ = 0; int connection_delay_ = 100; bool enable_sync_host_time_ = true; + std::string preset_firmware_path_; rclcpp::Service::SharedPtr reboot_device_srv_ = nullptr; std::chrono::time_point start_time_; std::string extension_path_; static backward::SignalHandling sh; // for stack trace + std::string upgrade_firmware_; }; } // namespace orbbec_camera diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index 89f54930..959aba0f 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -116,6 +116,12 @@ def generate_launch_description(): DeclareLaunchArgument('tf_publish_rate', default_value='0.0'), DeclareLaunchArgument('ir_info_url', default_value=''), DeclareLaunchArgument('color_info_url', default_value=''), + # Network device settings: default enumerate_net_device is set to true, which will automatically enumerate network devices + # If you do not want to automatically enumerate network devices, + # you can set enumerate_net_device to true, net_device_ip to the device's IP address, and net_device_port to the default value of 8090 + DeclareLaunchArgument("enumerate_net_device", default_value="false"), + DeclareLaunchArgument("net_device_ip", default_value=""), + DeclareLaunchArgument("net_device_port", default_value="0"), DeclareLaunchArgument('log_level', default_value='none'), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index 562b4e6f..b4465f66 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -31,6 +31,7 @@ #include // For std::put_time std::string g_camera_name = "orbbec_camera"; // Assuming this is declared elsewhere +std::string g_time_domain = "global"; // Assuming this is declared elsewhere void signalHandler(int sig) { std::cout << "Received signal: " << sig << std::endl; @@ -128,7 +129,11 @@ void OBCameraNodeDriver::init() { auto log_level = obLogSeverityFromString(log_level_str); connection_delay_ = static_cast(declare_parameter("connection_delay", 100)); enable_sync_host_time_ = declare_parameter("enable_sync_host_time", true); + upgrade_firmware_ = declare_parameter("upgrade_firmware", ""); g_camera_name = declare_parameter("camera_name", g_camera_name); + g_time_domain = declare_parameter("time_domain", g_time_domain); + preset_firmware_path_ = + declare_parameter("preset_firmware_path", preset_firmware_path_); ob::Context::setLoggerToConsole(log_level); orb_device_lock_shm_fd_ = shm_open(ORB_DEFAULT_LOCK_NAME.c_str(), O_CREAT | O_RDWR, 0666); if (orb_device_lock_shm_fd_ < 0) { @@ -246,7 +251,7 @@ void OBCameraNodeDriver::checkConnectTimer() { void OBCameraNodeDriver::queryDevice() { while (is_alive_ && rclcpp::ok() && !device_connected_.load()) { - if (!net_device_ip_.empty() && net_device_port_ != 0) { + if (!enumerate_net_device_ && !net_device_ip_.empty() && net_device_port_ != 0) { connectNetDevice(net_device_ip_, net_device_port_); } else { auto device_list = ctx_->queryDeviceList(); @@ -277,7 +282,6 @@ void OBCameraNodeDriver::resetDevice() { device_info_.reset(); device_connected_ = false; device_unique_id_.clear(); - serial_number_.clear(); reset_device_flag_ = false; } RCLCPP_INFO_STREAM(logger_, "Reset device uid: " << device_unique_id_ << " done"); @@ -302,7 +306,10 @@ void OBCameraNodeDriver::rebootDeviceCallback( std::shared_ptr OBCameraNodeDriver::selectDevice( const std::shared_ptr &list) { std::shared_ptr device = nullptr; - if (!serial_number_.empty()) { + if (!net_device_ip_.empty() && net_device_port_ != 0) { + RCLCPP_INFO_STREAM(logger_, "Connecting to device with net ip: " << net_device_ip_); + device = selectDeviceByNetIP(list, net_device_ip_); + } else if (!serial_number_.empty()) { RCLCPP_INFO_STREAM(logger_, "Connecting to device with serial number: " << serial_number_); device = selectDeviceBySerialNumber(list, serial_number_); } else if (!usb_port_.empty()) { @@ -388,8 +395,43 @@ std::shared_ptr OBCameraNodeDriver::selectDeviceByUSBPort( return nullptr; } +std::shared_ptr OBCameraNodeDriver::selectDeviceByNetIP( + const std::shared_ptr &list, const std::string &net_ip) { + RCLCPP_INFO_STREAM(logger_, "Before lock: Select device net ip: " << net_ip); + std::lock_guard lock(device_lock_); + RCLCPP_INFO_STREAM(logger_, "After lock: Select device net ip: " << net_ip); + std::shared_ptr device = nullptr; + for (size_t i = 0; i < list->getCount(); i++) { + try { + if (std::string(list->getConnectionType(i)) != "Ethernet") { + continue; + } + if (list->getIpAddress(i) == nullptr) { + continue; + } + RCLCPP_INFO_STREAM(logger_, "FindDeviceByNetIP device net ip " << list->getIpAddress(i)); + if (std::string(list->getIpAddress(i)) == net_ip) { + RCLCPP_INFO_STREAM(logger_, "getDeviceByNetIP device net ip " << net_ip << " done"); + return list->getDevice(i); + } + } catch (ob::Error &e) { + RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.getMessage()); + continue; + } catch (std::exception &e) { + RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.what()); + continue; + } catch (...) { + RCLCPP_ERROR_STREAM(logger_, "Failed to get device info"); + continue; + } + } + + return nullptr; +} + void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &device) { device_ = device; + updatePresetFirmware(preset_firmware_path_); CHECK_NOTNULL(device_); CHECK_NOTNULL(device_.get()); if (ob_camera_node_) { @@ -428,9 +470,6 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &dev " attempts."); } - ob_camera_node_->startIMU(); - ob_camera_node_->startStreams(); - device_connected_ = true; device_info_ = device_->getDeviceInfo(); serial_number_ = device_info_->getSerialNumber(); @@ -439,6 +478,13 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &dev if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid())) { TRY_EXECUTE_BLOCK(device_->timerSyncWithHost()); + if (g_time_domain != "global") { + sync_host_time_timer_ = this->create_wall_timer(std::chrono::milliseconds(60000), [this]() { + if (device_) { + TRY_EXECUTE_BLOCK(device_->timerSyncWithHost()); + } + }); + } } RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->getName() << " connected"); @@ -451,6 +497,20 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &dev auto time_cost = std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - start_time_); RCLCPP_INFO_STREAM(logger_, "Start device cost " << time_cost.count() << " ms"); + if (!upgrade_firmware_.empty()) { + device_->updateFirmware( + upgrade_firmware_.c_str(), + std::bind(&OBCameraNodeDriver::firmwareUpdateCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + false); + } + if (ob_camera_node_) { + ob_camera_node_->startIMU(); + ob_camera_node_->startStreams(); + } else { + RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ is nullptr"); + } + } // namespace orbbec_camera void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int net_device_port) { @@ -480,7 +540,30 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr &list device_.reset(); } std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_)); - pthread_mutex_lock(orb_device_lock_); + int try_lock_count = 0; + int max_try_lock_count = 5; + + while (try_lock_count < max_try_lock_count) { + int try_lock_result = pthread_mutex_trylock(orb_device_lock_); + + if (try_lock_result == 0) { + // success get lock,break + break; + } else if (try_lock_result == EBUSY) { + RCLCPP_INFO_STREAM(logger_, "Device lock is held by another process, waiting 100ms"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } else { + RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_"); + return; // Not EBUSY, return + } + + try_lock_count++; + } + if (try_lock_count >= max_try_lock_count) { + RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_"); + return; + } + std::shared_ptr lock_holder(nullptr, [this](int *) { pthread_mutex_unlock(orb_device_lock_); }); bool start_device_failed = false; @@ -523,6 +606,150 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr &list reset_device_cond_.notify_all(); } } +void OBCameraNodeDriver::updatePresetFirmware(std::string path) { + if (path.empty()) { + return; + } else { + std::stringstream ss(path); + std::string path_segment; + std::vector paths; + OBFwUpdateState updateState = STAT_START; + bool firstCall = true; + + while (std::getline(ss, path_segment, ',')) { + paths.push_back(path_segment); + } + uint8_t index = 0; + uint8_t count = static_cast(paths.size()); + char (*filePaths)[OB_PATH_MAX] = new char[count][OB_PATH_MAX]; + RCLCPP_INFO_STREAM(this->get_logger(), "paths.cout : " << (uint32_t)count); + for (const auto &p : paths) { + strcpy(filePaths[index], p.c_str()); + RCLCPP_INFO_STREAM(this->get_logger(), + "path: " << (uint32_t)index << ":" << filePaths[index]); + index++; + } + RCLCPP_INFO_STREAM(this->get_logger(), + "Start to update optional depth preset, please wait a moment..."); + try { + device_->updateOptionalDepthPresets( + filePaths, count, + [this, &updateState, &firstCall](OBFwUpdateState state, const char *message, + uint8_t percent) { + updateState = state; + presetUpdateCallback(firstCall, state, message, percent); + // firstCall = false; + }); + + delete[] filePaths; + filePaths = nullptr; + if (updateState == STAT_DONE || updateState == STAT_DONE_WITH_DUPLICATES) { + RCLCPP_INFO_STREAM(this->get_logger(), "After updating the preset: "); + auto presetList = device_->getAvailablePresetList(); + RCLCPP_INFO_STREAM(this->get_logger(), "Preset count: " << presetList->getCount()); + for (uint32_t i = 0; i < presetList->getCount(); ++i) { + RCLCPP_INFO_STREAM(this->get_logger(), " - " << presetList->getName(i)); + } + RCLCPP_INFO_STREAM(this->get_logger(), + "Current preset: " << device_->getCurrentPresetName()); + std::string key = "PresetVer"; + if (device_->isExtensionInfoExist(key)) { + std::string value = device_->getExtensionInfo(key); + RCLCPP_INFO_STREAM(this->get_logger(), "Preset version: " << value); + } else { + RCLCPP_INFO_STREAM(this->get_logger(), "PresetVer: "); + } + } + } catch (ob::Error &e) { + RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware " << e.getMessage()); + } catch (std::exception &e) { + RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware " << e.what()); + } catch (...) { + RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware"); + } + } +} +void OBCameraNodeDriver::presetUpdateCallback(bool firstCall, OBFwUpdateState state, + const char *message, uint8_t percent) { + if (!firstCall) { + std::cout << "\033[3F"; + } + + std::cout << "\033[K"; + std::cout << "Progress: " << static_cast(percent) << "%" << std::endl; + + std::cout << "\033[K"; + std::cout << "Status : "; + switch (state) { + case STAT_VERIFY_SUCCESS: + std::cout << "Image file verification success" << std::endl; + break; + case STAT_FILE_TRANSFER: + std::cout << "File transfer in progress" << std::endl; + break; + case STAT_DONE: + std::cout << "Update completed" << std::endl; + break; + case STAT_DONE_WITH_DUPLICATES: + std::cout << "Update completed, duplicated presets have been ignored" << std::endl; + break; + case STAT_IN_PROGRESS: + std::cout << "Update in progress" << std::endl; + break; + case STAT_START: + std::cout << "Starting the update" << std::endl; + break; + case STAT_VERIFY_IMAGE: + std::cout << "Verifying image file" << std::endl; + break; + default: + std::cout << "Unknown status or error" << std::endl; + break; + } + + std::cout << "\033[K"; + std::cout << "Message : " << message << std::endl << std::flush; +} +void OBCameraNodeDriver::firmwareUpdateCallback(OBFwUpdateState state, const char *message, + uint8_t percent) { + std::cout << "\033[K"; // Clear the current line + std::cout << "Progress: " << static_cast(percent) << "%" << std::endl; + + std::cout << "\033[K"; + std::cout << "Status : "; + switch (state) { + case STAT_VERIFY_SUCCESS: + std::cout << "Image file verification success" << std::endl; + break; + case STAT_FILE_TRANSFER: + std::cout << "File transfer in progress" << std::endl; + break; + case STAT_DONE: + std::cout << "Update completed" << std::endl; + break; + case STAT_IN_PROGRESS: + std::cout << "Upgrade in progress" << std::endl; + break; + case STAT_START: + std::cout << "Starting the upgrade" << std::endl; + break; + case STAT_VERIFY_IMAGE: + std::cout << "Verifying image file" << std::endl; + break; + default: + std::cout << "Unknown status or error" << std::endl; + break; + } + + std::cout << "\033[K"; + std::cout << "Message : " << message << std::endl << std::flush; + if (state == STAT_DONE) { + RCLCPP_INFO(logger_, "Reboot device"); + ob_camera_node_->rebootDevice(); + device_connected_ = false; + upgrade_firmware_ = ""; + } +} } // namespace orbbec_camera RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::OBCameraNodeDriver)