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

refactory startDevice

Browse Source
This commit is contained in:
Joe Dong
2023-02-17 20:18:55 +08:00
parent db5c017ed4
commit ef5218dc6d
2 changed files with 134 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h
+12
View File
@@ -14,6 +14,7 @@
#include <thread> #include <thread>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <semaphore.h>
#include "ob_camera_node.h" #include "ob_camera_node.h"
#include "utils.h" #include "utils.h"
#include "dynamic_params.h" #include "dynamic_params.h"
@@ -31,6 +32,17 @@ class OBCameraNodeFactory : public rclcpp::Node {
private: private:
void init(); void init();
void releaseDeviceSemaphore(sem_t* device_sem, size_t& num_devices_connected);
void updateConnectedDeviceCount(size_t& num_devices_connected);
std::shared_ptr<ob::Device> selectDevice(const std::shared_ptr<ob::DeviceList>& list);
std::shared_ptr<ob::Device> selectDeviceBySerialNumber(
const std::shared_ptr<ob::DeviceList>& list, const std::string& serial_number);
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);
void onDeviceConnected(const std::shared_ptr<ob::DeviceList>& device_list); void onDeviceConnected(const std::shared_ptr<ob::DeviceList>& device_list);
orbbec_camera/src/ob_camera_node_factory.cpp
+122 -101
View File
@@ -141,119 +141,119 @@ void OBCameraNodeFactory::queryDevice() {
} }
} }
void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &list) { void OBCameraNodeFactory::releaseDeviceSemaphore(sem_t *device_sem, size_t &num_devices_connected) {
std::scoped_lock<decltype(device_lock_)> lock(device_lock_); RCLCPP_INFO(logger_, "Release device semaphore");
if (device_connected_) { sem_post(device_sem);
return; int sem_value = 0;
sem_getvalue(device_sem, &sem_value);
RCLCPP_INFO_STREAM(logger_, "semaphore value: " << sem_value);
RCLCPP_INFO_STREAM(logger_, "Release device semaphore done");
if (num_devices_connected >= device_num_) {
RCLCPP_INFO_STREAM(logger_, "All devices connected, sem_unlink");
sem_destroy(device_sem);
sem_unlink(DEFAULT_SEM_NAME.c_str());
RCLCPP_INFO_STREAM(logger_, "All devices connected, sem_unlink done..");
} }
if (list->deviceCount() == 0) { }
RCLCPP_WARN(logger_, "No device found");
return; void OBCameraNodeFactory::updateConnectedDeviceCount(size_t &num_devices_connected) {
} // write connected device count to file
if (device_) { int shm_id = shmget(DEFAULT_SEM_KEY, 1, 0666 | IPC_CREAT);
device_.reset(); if (shm_id == -1) {
} RCLCPP_INFO_STREAM(logger_, "Failed to create shared memory " << strerror(errno));
size_t connected_device_num = 0; } else {
sem_t *device_sem = nullptr; RCLCPP_INFO_STREAM(logger_, "Created shared memory");
std::shared_ptr<int> sem_guard(nullptr, [&](int const *) { auto shm_ptr = (int *)shmat(shm_id, nullptr, 0);
if (device_num_ > 1 && device_sem) { if (shm_ptr == (void *)-1) {
RCLCPP_INFO(logger_, "Release device semaphore"); RCLCPP_INFO_STREAM(logger_, "Failed to attach shared memory " << strerror(errno));
sem_post(device_sem); } else {
int sem_value = 0; RCLCPP_INFO_STREAM(logger_, "Attached shared memory");
sem_getvalue(device_sem, &sem_value); num_devices_connected = *shm_ptr + 1;
RCLCPP_INFO_STREAM(logger_, "semaphore value: " << sem_value); RCLCPP_INFO_STREAM(logger_, "Current connected device " << num_devices_connected);
RCLCPP_INFO_STREAM(logger_, "Release device semaphore done"); *shm_ptr = static_cast<int>(num_devices_connected);
if (connected_device_num >= device_num_) { RCLCPP_INFO_STREAM(logger_, "Wrote to shared memory");
RCLCPP_INFO_STREAM(logger_, "All devices connected, sem_unlink"); shmdt(shm_ptr);
sem_destroy(device_sem); if (num_devices_connected >= device_num_) {
sem_unlink(DEFAULT_SEM_NAME.c_str()); RCLCPP_INFO_STREAM(logger_, "All devices connected, removing shared memory");
RCLCPP_INFO_STREAM(logger_, "All devices connected, sem_unlink done.."); shmctl(shm_id, IPC_RMID, nullptr);
} }
} }
}); }
}
std::shared_ptr<ob::Device> OBCameraNodeFactory::selectDevice(
const std::shared_ptr<ob::DeviceList> &list) {
if (device_num_ == 1) { if (device_num_ == 1) {
RCLCPP_INFO_STREAM(logger_, "Connecting to the default device"); RCLCPP_INFO_STREAM(logger_, "Connecting to the default device");
device_ = list->getDevice(0); return list->getDevice(0);
} else { }
std::string lower_sn; sem_t *device_sem = sem_open(DEFAULT_SEM_NAME.c_str(), O_CREAT, 0644, 1);
std::transform(serial_number_.begin(), serial_number_.end(), std::back_inserter(lower_sn), if (device_sem == SEM_FAILED) {
[](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); }); RCLCPP_INFO_STREAM(logger_, "Failed to open semaphore");
device_sem = sem_open(DEFAULT_SEM_NAME.c_str(), O_CREAT, 0644, 1); return nullptr;
if (device_sem == SEM_FAILED) { }
RCLCPP_INFO_STREAM(logger_, "Failed to open semaphore"); size_t num_devices_connected = 0;
return; std::shared_ptr<int> sem_guard(nullptr, [&](int const *) {
} releaseDeviceSemaphore(device_sem, num_devices_connected);
int sem_value = 0; updateConnectedDeviceCount(num_devices_connected);
sem_getvalue(device_sem, &sem_value); });
RCLCPP_INFO_STREAM(logger_, "semaphore value: " << sem_value); RCLCPP_INFO_STREAM(logger_, "Connecting to device with serial number: " << serial_number_);
if (int ret = sem_wait(device_sem); ret != 0) { int sem_value = 0;
RCLCPP_INFO_STREAM(logger_, "Failed to wait semaphore " << strerror(errno)); sem_getvalue(device_sem, &sem_value);
return; RCLCPP_INFO_STREAM(logger_, "semaphore value: " << sem_value);
} int ret = sem_wait(device_sem);
if (ret != 0) {
RCLCPP_ERROR_STREAM(logger_, "Failed to wait semaphore " << strerror(errno));
return nullptr;
}
auto device = selectDeviceBySerialNumber(list, serial_number_);
if (device == nullptr) {
RCLCPP_WARN(logger_, "Device with serial number %s not found", serial_number_.c_str());
device_connected_ = false;
return nullptr;
}
return device;
}
for (size_t i = 0; i < list->deviceCount(); i++) { std::shared_ptr<ob::Device> OBCameraNodeFactory::selectDeviceBySerialNumber(
try { const std::shared_ptr<ob::DeviceList> &list, const std::string &serial_number) {
auto pid = list->pid(i); std::string lower_sn;
if ((pid >= OPENNI_START_PID && pid <= OPENNI_END_PID) || pid == ASTRA_MINI_PID || std::transform(serial_number.begin(), serial_number.end(), std::back_inserter(lower_sn),
pid == ASTRA_MINI_S_PID) { [](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); });
// openNI device for (size_t i = 0; i < list->deviceCount(); i++) {
auto dev = list->getDevice(i); try {
auto device_info = dev->getDeviceInfo(); auto pid = list->pid(i);
if (device_info->serialNumber() == serial_number_) { if ((pid >= OPENNI_START_PID && pid <= OPENNI_END_PID) || pid == ASTRA_MINI_PID ||
RCLCPP_INFO_STREAM( pid == ASTRA_MINI_S_PID) {
logger_, "Device serial number " << device_info->serialNumber() << " matched"); // openNI device
device_ = dev; auto device = list->getDevice(i);
break; auto device_info = device->getDeviceInfo();
} if (device_info->serialNumber() == serial_number) {
} else { RCLCPP_INFO_STREAM(logger_,
std::string sn = list->serialNumber(i); "Device serial number " << device_info->serialNumber() << " matched");
RCLCPP_INFO_STREAM(logger_, "Device serial number: " << sn); return device;
if (sn == serial_number_) {
RCLCPP_INFO_STREAM(logger_, "Device serial number <<" << sn << " matched");
auto dev = list->getDevice(i);
device_ = dev;
break;
}
} }
} catch (ob::Error &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.getMessage());
} catch (std::exception &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.what());
} catch (...) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info");
}
}
if (device_ == nullptr) {
RCLCPP_WARN(logger_, "Device with serial number %s not found", serial_number_.c_str());
device_connected_ = false;
return;
} else {
// write connected device info to file
int shm_id = shmget(DEFAULT_SEM_KEY, 1, 0666 | IPC_CREAT);
if (shm_id == -1) {
RCLCPP_INFO_STREAM(logger_, "Failed to create shared memory " << strerror(errno));
} else { } else {
RCLCPP_INFO_STREAM(logger_, "Created shared memory"); std::string sn = list->serialNumber(i);
auto shm_ptr = (int *)shmat(shm_id, nullptr, 0); RCLCPP_INFO_STREAM(logger_, "Device serial number: " << sn);
if (shm_ptr == (void *)-1) { if (sn == serial_number) {
RCLCPP_INFO_STREAM(logger_, "Failed to attach shared memory " << strerror(errno)); RCLCPP_INFO_STREAM(logger_, "Device serial number <<" << sn << " matched");
} else { return list->getDevice(i);
RCLCPP_INFO_STREAM(logger_, "Attached shared memory");
connected_device_num = *shm_ptr + 1;
RCLCPP_INFO_STREAM(logger_, "Current connected device " << connected_device_num);
*shm_ptr = static_cast<int>(connected_device_num);
RCLCPP_INFO_STREAM(logger_, "Wrote to shared memory");
shmdt(shm_ptr);
if (connected_device_num >= device_num_) {
RCLCPP_INFO_STREAM(logger_, "All devices connected, removing shared memory");
shmctl(shm_id, IPC_RMID, nullptr);
}
} }
} }
} catch (ob::Error &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.getMessage());
} catch (std::exception &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.what());
} catch (...) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info");
} }
} }
return nullptr;
}
void OBCameraNodeFactory::initializeDevice(const std::shared_ptr<ob::Device> &device) {
device_ = device;
CHECK_NOTNULL(device_); CHECK_NOTNULL(device_);
CHECK_NOTNULL(device_.get()); CHECK_NOTNULL(device_.get());
if (ob_camera_node_) { if (ob_camera_node_) {
@@ -271,4 +271,25 @@ void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &lis
RCLCPP_INFO_STREAM(logger_, "device type: " << ObDeviceTypeToString(device_info_->deviceType())); RCLCPP_INFO_STREAM(logger_, "device type: " << ObDeviceTypeToString(device_info_->deviceType()));
RCLCPP_INFO_STREAM(logger_, "device unique id: " << device_unique_id_); RCLCPP_INFO_STREAM(logger_, "device unique id: " << device_unique_id_);
} }
void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &list) {
std::scoped_lock<decltype(device_lock_)> lock(device_lock_);
if (device_connected_) {
return;
}
if (list->deviceCount() == 0) {
RCLCPP_WARN(logger_, "No device found");
return;
}
if (device_) {
device_.reset();
}
auto device = selectDevice(list);
if (device == nullptr) {
RCLCPP_WARN(logger_, "Device with serial number %s not found", serial_number_.c_str());
device_connected_ = false;
return;
}
initializeDevice(device);
}
} // namespace orbbec_camera } // namespace orbbec_camera