i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-05 04:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

add read device usb port

Browse Source
This commit is contained in:
Joe Dong
2023-07-05 15:00:34 +08:00
parent 8c6d0212e1
commit 15ca0e347b
11 changed files with 134 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node_driver.cpp
+59 -4
View File
@@ -12,7 +12,6 @@
#include "orbbec_camera/ob_camera_node_driver.h"
#include <fcntl.h>
#include <unistd.h>
#include <semaphore.h>
#include <sys/shm.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
@@ -44,6 +43,9 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
if (device_count_update_thread_ && device_count_update_thread_->joinable()) {
device_count_update_thread_->join();
}
if (sync_time_thread_ && sync_time_thread_->joinable()) {
sync_time_thread_->join();
}
if (query_thread_ && query_thread_->joinable()) {
query_thread_->join();
}
@@ -57,8 +59,8 @@ void OBCameraNodeDriver::init() {
parameters_ = std::make_shared<Parameters>(this);
serial_number_ = declare_parameter<std::string>("serial_number", "");
device_num_ = static_cast<int>(declare_parameter<int>("device_num", 1));
ctx_->setDeviceChangedCallback([this](std::shared_ptr<ob::DeviceList> removed_list,
std::shared_ptr<ob::DeviceList> added_list) {
ctx_->setDeviceChangedCallback([this](const std::shared_ptr<ob::DeviceList> &removed_list,
const std::shared_ptr<ob::DeviceList> &added_list) {
(void)added_list;
onDeviceDisconnected(removed_list);
});
@@ -67,6 +69,7 @@ void OBCameraNodeDriver::init() {
CHECK_NOTNULL(check_connect_timer_);
query_thread_ = std::make_shared<std::thread>([this]() { queryDevice(); });
device_count_update_thread_ = std::make_shared<std::thread>([this]() { deviceCountUpdate(); });
sync_time_thread_ = std::make_shared<std::thread>([this]() { syncTime(); });
CHECK_NOTNULL(device_count_update_thread_);
}
@@ -164,6 +167,13 @@ void OBCameraNodeDriver::deviceCountUpdate() {
}
}
void OBCameraNodeDriver::syncTime() {
while (is_alive_ && rclcpp::ok()) {
ctx_->enableMultiDeviceSync(0);
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
}
}
void OBCameraNodeDriver::releaseDeviceSemaphore(sem_t *device_sem, int &num_devices_connected) {
RCLCPP_INFO_THROTTLE(logger_, *get_clock(), 1000, "Release device semaphore");
sem_post(device_sem);
@@ -230,7 +240,16 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
releaseDeviceSemaphore(device_sem, num_devices_connected_);
return nullptr;
}
auto device = selectDeviceBySerialNumber(list, serial_number_);
std::shared_ptr<ob::Device> device = nullptr;
if (!serial_number_.empty()) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000,
"Connecting to device with serial number: " << serial_number_);
device = selectDeviceBySerialNumber(list, serial_number_);
} else if (!usb_port_.empty()) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000,
"Connecting to device with usb port: " << usb_port_);
device = selectDeviceByUSBPort(list, usb_port_);
}
std::shared_ptr<int> sem_guard(nullptr, [&, device](int const *) {
auto connect_event = device != nullptr ? DeviceConnectionEvent::kDeviceConnected
: DeviceConnectionEvent::kOtherDeviceConnected;
@@ -284,6 +303,41 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
return nullptr;
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
const std::shared_ptr<ob::DeviceList> &list, const std::string &usb_port) {
for (size_t i = 0; i < list->deviceCount(); i++) {
try {
auto pid = list->pid(i);
if (isOpenNIDevice(pid)) {
// openNI device
auto dev = list->getDevice(i);
auto device_info = dev->getDeviceInfo();
std::string uid = device_info->uid();
auto port_id = parseUsbPort(uid);
if (port_id == usb_port) {
RCLCPP_INFO_STREAM(logger_, "Device port id " << port_id << " matched");
return dev;
}
} else {
std::string uid = list->uid(i);
auto port_id = parseUsbPort(uid);
RCLCPP_INFO_STREAM(logger_, "Device usb port: " << uid);
if (port_id == usb_port) {
RCLCPP_INFO_STREAM(logger_, "Device usb port <<" << uid << " matched");
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) {
device_ = device;
CHECK_NOTNULL(device_);
@@ -296,6 +350,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
device_info_ = device_->getDeviceInfo();
CHECK_NOTNULL(device_info_.get());
device_unique_id_ = device_info_->uid();
ctx_->enableMultiDeviceSync(0); // sync time stamp
RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->name() << " connected");
RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->serialNumber());
RCLCPP_INFO_STREAM(logger_, "Firmware version: " << device_info_->firmwareVersion());