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

Fix connect net device bug

Browse Source
This commit is contained in:
jj
2025-04-02 18:37:02 +08:00
parent e515442bc7
commit e289b60bc4
2 changed files with 49 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
+3
View File
@@ -48,6 +48,9 @@ class OBCameraNodeDriver : public rclcpp::Node {
std::shared_ptr<ob::Device> selectDeviceByUSBPort(const std::shared_ptr<ob::DeviceList>& list, std::shared_ptr<ob::Device> selectDeviceByUSBPort(const std::shared_ptr<ob::DeviceList>& list,
const std::string& usb_port); 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 initializeDevice(const std::shared_ptr<ob::Device>& device);
void startDevice(const std::shared_ptr<ob::DeviceList>& list); void startDevice(const std::shared_ptr<ob::DeviceList>& list);
orbbec_camera/src/ob_camera_node_driver.cpp
+46 -2
View File
@@ -250,7 +250,10 @@ void OBCameraNodeDriver::checkConnectTimer() {
void OBCameraNodeDriver::queryDevice() { void OBCameraNodeDriver::queryDevice() {
while (is_alive_ && rclcpp::ok() && !device_connected_.load()) { while (is_alive_ && rclcpp::ok() && !device_connected_.load()) {
if (!net_device_ip_.empty() && net_device_port_ != 0) { RCLCPP_INFO_STREAM(logger_, "enumerate_net_device: " << enumerate_net_device_);
RCLCPP_INFO_STREAM(logger_, "net_device_ip_: " << net_device_ip_);
RCLCPP_INFO_STREAM(logger_, "net_device_port_: " << net_device_port_);
if (!enumerate_net_device_ && !net_device_ip_.empty() && net_device_port_ != 0) {
connectNetDevice(net_device_ip_, net_device_port_); connectNetDevice(net_device_ip_, net_device_port_);
} else { } else {
auto device_list = ctx_->queryDeviceList(); auto device_list = ctx_->queryDeviceList();
@@ -305,7 +308,10 @@ void OBCameraNodeDriver::rebootDeviceCallback(
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice( std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
const std::shared_ptr<ob::DeviceList> &list) { const std::shared_ptr<ob::DeviceList> &list) {
std::shared_ptr<ob::Device> device = nullptr; 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_); RCLCPP_INFO_STREAM(logger_, "Connecting to device with serial number: " << serial_number_);
device = selectDeviceBySerialNumber(list, serial_number_); device = selectDeviceBySerialNumber(list, serial_number_);
} else if (!usb_port_.empty()) { } else if (!usb_port_.empty()) {
@@ -391,6 +397,44 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
return nullptr; return nullptr;
} }
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
const std::shared_ptr<ob::DeviceList> &list, const std::string &net_ip) {
try {
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 {
device = list->getDevice(i);
} catch (ob::Error &e) {
continue;
} catch (std::exception &e) {
continue;
} catch (...) {
continue;
}
auto device_info = device->getDeviceInfo();
RCLCPP_INFO_STREAM(logger_, "FindDeviceByNetIP device net ip " << net_ip);
if (device_info->getIpAddress() == nullptr) {
continue;
}
if (std::string(device_info->getIpAddress()) == 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());
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info");
}
return nullptr;
}
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) { void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
device_ = device; device_ = device;
updatePresetFirmware(preset_firmware_path_); updatePresetFirmware(preset_firmware_path_);