i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-07 21:37: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/list_camera_profile.cpp
+1
View File
@@ -3,6 +3,7 @@
#include <orbbec_camera/ob_camera_node_driver.h>
#include <memory>
#include <magic_enum/magic_enum.hpp>
int main() {
auto context = std::make_unique<ob::Context>();
context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
orbbec_camera/src/list_devices_node.cpp
+7 -1
View File
@@ -1,14 +1,20 @@
#include <rclcpp/rclcpp.hpp>
#include <orbbec_camera/ob_camera_node_driver.h>
#include <orbbec_camera/utils.h>
int main() {
auto context = std::make_unique<ob::Context>();
context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
auto list = context->queryDeviceList();
for (size_t i = 0; i < list->deviceCount(); i++) {
auto serial = list->getDevice(i)->getDeviceInfo()->serialNumber();
auto device = list->getDevice(i);
auto device_info = device->getDeviceInfo();
std::string serial = device_info->serialNumber();
std::string uid = device_info->uid();
auto usb_port = orbbec_camera::parseUsbPort(uid);
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "serial: " << serial);
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "usb port: " << usb_port);
}
return 0;
}
orbbec_camera/src/ob_camera_node.cpp
+4 -4
View File
@@ -215,7 +215,7 @@ void OBCameraNode::startStreams() {
pipeline_ = std::make_unique<ob::Pipeline>(device_);
try {
setupPipelineConfig();
pipeline_->start(pipeline_config_, [this](std::shared_ptr<ob::FrameSet> frame_set) {
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet>& frame_set) {
onNewFrameSetCallback(frame_set);
});
} catch (const ob::Error& e) {
@@ -223,7 +223,7 @@ void OBCameraNode::startStreams() {
RCLCPP_INFO_STREAM(logger_, "try to disable ir stream and try again");
enable_stream_[INFRA0] = false;
setupPipelineConfig();
pipeline_->start(pipeline_config_, [this](std::shared_ptr<ob::FrameSet> frame_set) {
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet>& frame_set) {
onNewFrameSetCallback(frame_set);
});
}
@@ -244,7 +244,7 @@ void OBCameraNode::startIMU() {
auto accel_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
if (profile->fullScaleRange() == accel_range && profile->sampleRate() == accel_rate) {
sensors_[stream_index]->start(profile,
[this, stream_index](std::shared_ptr<ob::Frame> frame) {
[this, stream_index](const std::shared_ptr<ob::Frame>& frame) {
onNewIMUFrameCallback(frame, stream_index);
});
imu_started_[stream_index] = true;
@@ -258,7 +258,7 @@ void OBCameraNode::startIMU() {
auto gyro_range = fullGyroScaleRangeFromString(imu_range_[stream_index]);
if (profile->fullScaleRange() == gyro_range && profile->sampleRate() == gyro_rate) {
sensors_[stream_index]->start(profile,
[this, stream_index](std::shared_ptr<ob::Frame> frame) {
[this, stream_index](const std::shared_ptr<ob::Frame>& frame) {
onNewIMUFrameCallback(frame, stream_index);
});
RCLCPP_INFO_STREAM(logger_, "start gyro stream with "
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());
orbbec_camera/src/utils.cpp
+21
View File
@@ -10,6 +10,7 @@
/* */
/**************************************************************************/
#include <regex>
#include "orbbec_camera/utils.h"
namespace orbbec_camera {
sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
@@ -384,4 +385,24 @@ OBAccelFullScaleRange fullAccelScaleRangeFromString(std::string &full_scale_rang
return OB_ACCEL_FS_16g;
}
}
std::string parseUsbPort(const std::string &line) {
std::string port_id;
std::regex self_regex("(?:[^ ]+/usb[0-9]+[0-9./-]*/){0,1}([0-9.-]+)(:){0,1}[^ ]*",
std::regex_constants::ECMAScript);
std::smatch base_match;
bool found = std::regex_match(line, base_match, self_regex);
if (found) {
port_id = base_match[1].str();
if (base_match[2].str().empty()) // This is libuvc string. Remove counter is exists.
{
std::regex end_regex = std::regex(".+(-[0-9]+$)", std::regex_constants::ECMAScript);
bool found_end = std::regex_match(port_id, base_match, end_regex);
if (found_end) {
port_id = port_id.substr(0, port_id.size() - base_match[1].str().size());
}
}
}
return port_id;
}
} // namespace orbbec_camera