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

fixed device count

Browse Source
This commit is contained in:
Joe Dong
2023-02-17 22:25:24 +08:00
parent ef5218dc6d
commit c4f143cc82
2 changed files with 81 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h
+18 -3
View File
@@ -22,6 +22,15 @@
#include "libobsensor/ObSensor.hpp" #include "libobsensor/ObSensor.hpp"
namespace orbbec_camera { namespace orbbec_camera {
enum DeviceConnectionEvent {
kDeviceConnected = 0,
kDeviceDisconnected,
kOtherDeviceConnected,
kOtherDeviceDisconnected,
kOtherDeviceCountUpdate,
};
class OBCameraNodeFactory : public rclcpp::Node { class OBCameraNodeFactory : public rclcpp::Node {
public: public:
explicit OBCameraNodeFactory(const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()); explicit OBCameraNodeFactory(const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions());
@@ -32,9 +41,10 @@ class OBCameraNodeFactory : public rclcpp::Node {
private: private:
void init(); void init();
void releaseDeviceSemaphore(sem_t* device_sem, size_t& num_devices_connected); void releaseDeviceSemaphore(sem_t* device_sem, int& num_devices_connected);
void updateConnectedDeviceCount(size_t& num_devices_connected); void updateConnectedDeviceCount(int& num_devices_connected,
DeviceConnectionEvent connection_event);
std::shared_ptr<ob::Device> selectDevice(const std::shared_ptr<ob::DeviceList>& list); std::shared_ptr<ob::Device> selectDevice(const std::shared_ptr<ob::DeviceList>& list);
@@ -55,6 +65,9 @@ class OBCameraNodeFactory : public rclcpp::Node {
void queryDevice(); void queryDevice();
void getConnectedDeviceCountCallback(const std::shared_ptr<GetInt32::Request> request,
std::shared_ptr<GetInt32::Response> response);
private: private:
std::unique_ptr<ob::Context> ctx_ = nullptr; std::unique_ptr<ob::Context> ctx_ = nullptr;
rclcpp::Logger logger_; rclcpp::Logger logger_;
@@ -68,7 +81,9 @@ class OBCameraNodeFactory : public rclcpp::Node {
std::shared_ptr<Parameters> parameters_ = nullptr; std::shared_ptr<Parameters> parameters_ = nullptr;
std::shared_ptr<std::thread> query_thread_ = nullptr; std::shared_ptr<std::thread> query_thread_ = nullptr;
std::recursive_mutex device_lock_; std::recursive_mutex device_lock_;
size_t device_num_ = 1; int device_num_ = 1;
int num_devices_connected_ = 0;
rclcpp::TimerBase::SharedPtr check_connect_timer_ = nullptr; rclcpp::TimerBase::SharedPtr check_connect_timer_ = nullptr;
rclcpp::Service<GetInt32>::SharedPtr get_connected_device_count_srv_ = nullptr;
}; };
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera/src/ob_camera_node_factory.cpp
+63 -37
View File
@@ -50,7 +50,7 @@ void OBCameraNodeFactory::init() {
is_alive_.store(true); is_alive_.store(true);
parameters_ = std::make_shared<Parameters>(this); parameters_ = std::make_shared<Parameters>(this);
serial_number_ = declare_parameter<std::string>("serial_number", ""); serial_number_ = declare_parameter<std::string>("serial_number", "");
device_num_ = declare_parameter<int>("device_num", 1); device_num_ = static_cast<int>(declare_parameter<int>("device_num", 1));
ctx_->setDeviceChangedCallback([this](std::shared_ptr<ob::DeviceList> removed_list, ctx_->setDeviceChangedCallback([this](std::shared_ptr<ob::DeviceList> removed_list,
std::shared_ptr<ob::DeviceList> added_list) { std::shared_ptr<ob::DeviceList> added_list) {
(void)added_list; (void)added_list;
@@ -59,6 +59,14 @@ void OBCameraNodeFactory::init() {
check_connect_timer_ = check_connect_timer_ =
this->create_wall_timer(std::chrono::milliseconds(1000), [this]() { checkConnectTimer(); }); this->create_wall_timer(std::chrono::milliseconds(1000), [this]() { checkConnectTimer(); });
CHECK_NOTNULL(check_connect_timer_); CHECK_NOTNULL(check_connect_timer_);
get_connected_device_count_srv_ = this->create_service<GetInt32>(
"get_connected_device_count", [this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<GetInt32::Request> request,
const std::shared_ptr<GetInt32::Response> response) {
(void)request_header;
(void)request;
response->data = num_devices_connected_;
});
query_thread_ = std::make_shared<std::thread>([this]() { queryDevice(); }); query_thread_ = std::make_shared<std::thread>([this]() { queryDevice(); });
} }
@@ -87,6 +95,7 @@ void OBCameraNodeFactory::onDeviceDisconnected(const std::shared_ptr<ob::DeviceL
return; return;
} }
RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected"); RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected");
bool current_device_disconnected = false;
for (size_t i = 0; i < device_list->deviceCount(); i++) { for (size_t i = 0; i < device_list->deviceCount(); i++) {
std::string uid = device_list->uid(i); std::string uid = device_list->uid(i);
std::scoped_lock<decltype(device_lock_)> lock(device_lock_); std::scoped_lock<decltype(device_lock_)> lock(device_lock_);
@@ -95,10 +104,15 @@ void OBCameraNodeFactory::onDeviceDisconnected(const std::shared_ptr<ob::DeviceL
ob_camera_node_.reset(); ob_camera_node_.reset();
device_.reset(); device_.reset();
device_connected_ = false; device_connected_ = false;
current_device_disconnected = true;
device_unique_id_.clear(); device_unique_id_.clear();
break; break;
} }
} }
auto connect_event = current_device_disconnected
? DeviceConnectionEvent::kDeviceDisconnected
: DeviceConnectionEvent::kOtherDeviceDisconnected;
updateConnectedDeviceCount(num_devices_connected_, connect_event);
} }
OBLogSeverity OBCameraNodeFactory::obLogSeverityFromString(const std::string_view &log_level) { OBLogSeverity OBCameraNodeFactory::obLogSeverityFromString(const std::string_view &log_level) {
@@ -136,48 +150,54 @@ void OBCameraNodeFactory::queryDevice() {
} }
onDeviceConnected(device_list); onDeviceConnected(device_list);
} else { } else {
updateConnectedDeviceCount(num_devices_connected_,
DeviceConnectionEvent::kOtherDeviceCountUpdate);
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::this_thread::sleep_for(std::chrono::milliseconds(1000));
} }
} }
} }
void OBCameraNodeFactory::releaseDeviceSemaphore(sem_t *device_sem, size_t &num_devices_connected) { void OBCameraNodeFactory::releaseDeviceSemaphore(sem_t *device_sem, int &num_devices_connected) {
RCLCPP_INFO(logger_, "Release device semaphore"); RCLCPP_INFO_THROTTLE(logger_, *get_clock(), 1000, "Release device semaphore");
sem_post(device_sem); sem_post(device_sem);
int sem_value = 0; int sem_value = 0;
sem_getvalue(device_sem, &sem_value); sem_getvalue(device_sem, &sem_value);
RCLCPP_INFO_STREAM(logger_, "semaphore value: " << sem_value); RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "semaphore value: " << sem_value);
RCLCPP_INFO_STREAM(logger_, "Release device semaphore done"); RCLCPP_INFO_THROTTLE(logger_, *get_clock(), 1000, "Release device semaphore done");
if (num_devices_connected >= device_num_) { if (num_devices_connected >= device_num_) {
RCLCPP_INFO_STREAM(logger_, "All devices connected, sem_unlink");
sem_destroy(device_sem); sem_destroy(device_sem);
sem_unlink(DEFAULT_SEM_NAME.c_str()); sem_unlink(DEFAULT_SEM_NAME.c_str());
RCLCPP_INFO_STREAM(logger_, "All devices connected, sem_unlink done..");
} }
} }
void OBCameraNodeFactory::updateConnectedDeviceCount(size_t &num_devices_connected) { void OBCameraNodeFactory::updateConnectedDeviceCount(int &num_devices_connected,
DeviceConnectionEvent connection_event) {
// write connected device count to file // write connected device count to file
int shm_id = shmget(DEFAULT_SEM_KEY, 1, 0666 | IPC_CREAT); int shm_id = shmget(DEFAULT_SEM_KEY, 1, 0666 | IPC_CREAT);
if (shm_id == -1) { if (shm_id == -1) {
RCLCPP_INFO_STREAM(logger_, "Failed to create shared memory " << strerror(errno)); RCLCPP_INFO_STREAM(logger_, "Failed to create shared memory " << strerror(errno));
return;
}
auto shm_ptr = (int *)shmat(shm_id, nullptr, 0);
if (shm_ptr == (void *)-1) {
RCLCPP_INFO_STREAM(logger_, "Failed to attach shared memory " << strerror(errno));
return;
}
if (connection_event == DeviceConnectionEvent::kDeviceConnected) {
num_devices_connected = *shm_ptr + 1;
} else if (connection_event == DeviceConnectionEvent::kDeviceDisconnected && *shm_ptr > 0) {
num_devices_connected = *shm_ptr - 1;
} else { } else {
RCLCPP_INFO_STREAM(logger_, "Created shared memory"); num_devices_connected = *shm_ptr;
auto shm_ptr = (int *)shmat(shm_id, nullptr, 0); }
if (shm_ptr == (void *)-1) { RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
RCLCPP_INFO_STREAM(logger_, "Failed to attach shared memory " << strerror(errno)); "Current connected device " << num_devices_connected);
} else { *shm_ptr = static_cast<int>(num_devices_connected);
RCLCPP_INFO_STREAM(logger_, "Attached shared memory"); shmdt(shm_ptr);
num_devices_connected = *shm_ptr + 1; if (connection_event == DeviceConnectionEvent::kDeviceDisconnected &&
RCLCPP_INFO_STREAM(logger_, "Current connected device " << num_devices_connected); num_devices_connected == 0) {
*shm_ptr = static_cast<int>(num_devices_connected); shmctl(shm_id, IPC_RMID, nullptr);
RCLCPP_INFO_STREAM(logger_, "Wrote to shared memory"); sem_unlink(DEFAULT_SEM_NAME.c_str());
shmdt(shm_ptr);
if (num_devices_connected >= device_num_) {
RCLCPP_INFO_STREAM(logger_, "All devices connected, removing shared memory");
shmctl(shm_id, IPC_RMID, nullptr);
}
}
} }
} }
@@ -192,23 +212,27 @@ std::shared_ptr<ob::Device> OBCameraNodeFactory::selectDevice(
RCLCPP_INFO_STREAM(logger_, "Failed to open semaphore"); RCLCPP_INFO_STREAM(logger_, "Failed to open semaphore");
return nullptr; return nullptr;
} }
size_t num_devices_connected = 0; RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000,
std::shared_ptr<int> sem_guard(nullptr, [&](int const *) { "Connecting to device with serial number: " << serial_number_);
releaseDeviceSemaphore(device_sem, num_devices_connected);
updateConnectedDeviceCount(num_devices_connected);
});
RCLCPP_INFO_STREAM(logger_, "Connecting to device with serial number: " << serial_number_);
int sem_value = 0; int sem_value = 0;
sem_getvalue(device_sem, &sem_value); sem_getvalue(device_sem, &sem_value);
RCLCPP_INFO_STREAM(logger_, "semaphore value: " << sem_value); RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "semaphore value: " << sem_value);
int ret = sem_wait(device_sem); int ret = sem_wait(device_sem);
if (ret != 0) { if (ret != 0) {
RCLCPP_ERROR_STREAM(logger_, "Failed to wait semaphore " << strerror(errno)); RCLCPP_ERROR_STREAM(logger_, "Failed to wait semaphore " << strerror(errno));
return nullptr; return nullptr;
} }
auto device = selectDeviceBySerialNumber(list, serial_number_); auto device = selectDeviceBySerialNumber(list, serial_number_);
std::shared_ptr<int> sem_guard(nullptr, [&, device](int const *) {
auto connect_event = device != nullptr ? DeviceConnectionEvent::kDeviceConnected
: DeviceConnectionEvent::kOtherDeviceConnected;
updateConnectedDeviceCount(num_devices_connected_, connect_event);
releaseDeviceSemaphore(device_sem, num_devices_connected_);
});
if (device == nullptr) { if (device == nullptr) {
RCLCPP_WARN(logger_, "Device with serial number %s not found", serial_number_.c_str()); RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Device with serial number %s not found",
serial_number_.c_str());
device_connected_ = false; device_connected_ = false;
return nullptr; return nullptr;
} }
@@ -235,18 +259,19 @@ std::shared_ptr<ob::Device> OBCameraNodeFactory::selectDeviceBySerialNumber(
} }
} else { } else {
std::string sn = list->serialNumber(i); std::string sn = list->serialNumber(i);
RCLCPP_INFO_STREAM(logger_, "Device serial number: " << sn); RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "Device serial number: " << sn);
if (sn == serial_number) { if (sn == serial_number) {
RCLCPP_INFO_STREAM(logger_, "Device serial number <<" << sn << " matched"); RCLCPP_INFO_STREAM(logger_, "Device serial number <<" << sn << " matched");
return list->getDevice(i); return list->getDevice(i);
} }
} }
} catch (ob::Error &e) { } catch (ob::Error &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.getMessage()); RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 1000,
"Failed to get device info " << e.getMessage());
} catch (std::exception &e) { } catch (std::exception &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.what()); RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.what());
} catch (...) { } catch (...) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info"); RCLCPP_ERROR_STREAM(logger_, "Failed to get device info");
} }
} }
return nullptr; return nullptr;
@@ -286,7 +311,8 @@ void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &lis
} }
auto device = selectDevice(list); auto device = selectDevice(list);
if (device == nullptr) { if (device == nullptr) {
RCLCPP_WARN(logger_, "Device with serial number %s not found", serial_number_.c_str()); RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Device with serial number %s not found",
serial_number_.c_str());
device_connected_ = false; device_connected_ = false;
return; return;
} }