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

Update net device connection

Browse Source
This commit is contained in:
jj
2025-05-19 10:40:35 +08:00
parent 8ee93124f9
commit 5ea21060a7
3 changed files with 250 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
+10
View File
@@ -48,6 +48,9 @@ class OBCameraNodeDriver : public rclcpp::Node {
std::shared_ptr<ob::Device> selectDeviceByUSBPort(const std::shared_ptr<ob::DeviceList>& list,
const std::string& usb_port);
std::shared_ptr<ob::Device> selectDeviceByNetIP(const std::shared_ptr<ob::DeviceList>& list,
const std::string& net_ip);
void initializeDevice(const std::shared_ptr<ob::Device>& device);
void startDevice(const std::shared_ptr<ob::DeviceList>& list);
@@ -68,6 +71,11 @@ class OBCameraNodeDriver : public rclcpp::Node {
void rebootDeviceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response);
void 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<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
std::chrono::time_point<std::chrono::system_clock> start_time_;
std::string extension_path_;
static backward::SignalHandling sh; // for stack trace
std::string upgrade_firmware_;
};
} // namespace orbbec_camera
orbbec_camera/launch/gemini_330_series.launch.py
+6
View File
@@ -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'),
orbbec_camera/src/ob_camera_node_driver.cpp
+234 -7
View File
@@ -31,6 +31,7 @@
#include <iomanip> // 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<int>(declare_parameter<int>("connection_delay", 100));
enable_sync_host_time_ = declare_parameter<bool>("enable_sync_host_time", true);
upgrade_firmware_ = declare_parameter<std::string>("upgrade_firmware", "");
g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
g_time_domain = declare_parameter<std::string>("time_domain", g_time_domain);
preset_firmware_path_ =
declare_parameter<std::string>("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<ob::Device> OBCameraNodeDriver::selectDevice(
const std::shared_ptr<ob::DeviceList> &list) {
std::shared_ptr<ob::Device> 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<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
return nullptr;
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
const std::shared_ptr<ob::DeviceList> &list, const std::string &net_ip) {
RCLCPP_INFO_STREAM(logger_, "Before lock: Select device net ip: " << net_ip);
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM(logger_, "After lock: Select device net ip: " << net_ip);
std::shared_ptr<ob::Device> 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<ob::Device> &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<ob::Device> &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<ob::Device> &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<ob::Device> &dev
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(
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<ob::DeviceList> &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<int> 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<ob::DeviceList> &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<std::string> 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<uint8_t>(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<uint32_t>(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<uint32_t>(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)