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

add reboot device inteface

Browse Source
This commit is contained in:
Joe Dong
2024-07-29 09:50:02 +08:00
parent 709d59987f
commit af1e426a81
4 changed files with 32 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node_driver.cpp
+18
View File
@@ -79,6 +79,9 @@ void OBCameraNodeDriver::init() {
RCLCPP_ERROR_STREAM(logger_, "Failed to map shared memory " << ORB_DEFAULT_LOCK_NAME);
return;
}
reboot_device_srv_ = this->create_service<std_srvs::srv::Empty>(
"reboot_device", std::bind(&OBCameraNodeDriver::rebootDeviceCallback, this,
std::placeholders::_1, std::placeholders::_2));
pthread_mutexattr_init(&orb_device_lock_attr_);
pthread_mutexattr_setpshared(&orb_device_lock_attr_, PTHREAD_PROCESS_SHARED);
orb_device_lock_ = (pthread_mutex_t *)orb_device_lock_shm_addr_;
@@ -202,6 +205,21 @@ void OBCameraNodeDriver::resetDevice() {
}
}
void OBCameraNodeDriver::rebootDeviceCallback(
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response) {
(void)request;
(void)response;
if (!device_connected_) {
RCLCPP_WARN(logger_, "Device not connected");
return;
}
RCLCPP_INFO(logger_, "Reboot device");
ob_camera_node_->rebootDevice();
device_connected_ = false;
device_ = nullptr;
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
const std::shared_ptr<ob::DeviceList> &list) {
if (device_num_ == 1) {