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

fixed stop device

Browse Source
This commit is contained in:
Joe Dong
2022-06-08 15:33:16 +08:00
parent c6ded28f8e
commit 9f24dc1e45
2 changed files with 30 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h
+7 -2
View File
@@ -30,9 +30,13 @@ class OBCameraNodeFactory : public rclcpp::Node {
private: private:
void init(); void init();
void startDevice(); void startDevice();
void getDevice(const std::shared_ptr<ob::DeviceList>& list); void getDevice(const std::shared_ptr<ob::DeviceList>& list);
void updateDeviceInfo();
void deviceConnectCallback(const std::shared_ptr<ob::DeviceList>& device_list); void deviceConnectCallback(const std::shared_ptr<ob::DeviceList>& device_list);
void deviceDisconnectCallback(const std::shared_ptr<ob::DeviceList>& device_list); void deviceDisconnectCallback(const std::shared_ptr<ob::DeviceList>& device_list);
@@ -44,12 +48,13 @@ class OBCameraNodeFactory : public rclcpp::Node {
rclcpp::Logger logger_; rclcpp::Logger logger_;
std::unique_ptr<OBCameraNode> ob_camera_node_; std::unique_ptr<OBCameraNode> ob_camera_node_;
std::shared_ptr<ob::Device> device_; std::shared_ptr<ob::Device> device_;
std::shared_ptr<ob::DeviceInfo> device_info_;
std::atomic_bool is_alive_{false}; std::atomic_bool is_alive_{false};
std::thread query_thread_; std::thread query_thread_;
std::string serial_number_; std::string serial_number_;
std::string usb_port_id_; std::string usb_port_id_;
double reconnect_timeout_; double reconnect_timeout_ = 0.0;
double wait_for_device_timeout_; double wait_for_device_timeout_ = 0.0;
std::shared_ptr<Parameters> parameters_; std::shared_ptr<Parameters> parameters_;
}; };
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera/src/ob_camera_node_factory.cpp
+23 -16
View File
@@ -37,6 +37,8 @@ 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", "");
wait_for_device_timeout_ = declare_parameter<double>("wait_for_device_timeout", 2.0);
reconnect_timeout_ = declare_parameter<double>("reconnect_timeout", 2.0);
query_thread_ = std::thread([=]() { query_thread_ = std::thread([=]() {
std::chrono::milliseconds timespan(static_cast<int>(reconnect_timeout_ * 1e3)); std::chrono::milliseconds timespan(static_cast<int>(reconnect_timeout_ * 1e3));
rclcpp::Time first_try_time = this->now(); rclcpp::Time first_try_time = this->now();
@@ -96,22 +98,18 @@ void OBCameraNodeFactory::deviceDisconnectCallback(
} }
RCLCPP_ERROR_STREAM(logger_, "deviceDisconnectCallback"); RCLCPP_ERROR_STREAM(logger_, "deviceDisconnectCallback");
CHECK_NOTNULL(device_list); CHECK_NOTNULL(device_list);
ob_camera_node_.reset(nullptr); try {
device_.reset(); for (size_t i = 0; i < device_list->deviceCount(); i++) {
// try { std::string serial_number = device_list->serialNumber(i);
// for (size_t i = 0; i < device_list->deviceCount(); i++) { std::string device_serial_no = device_info_->serialNumber();
// auto dev = device_list->getDevice(i); if (serial_number == device_serial_no) {
// std::string sn1 = dev->getDeviceInfo()->serialNumber(); ob_camera_node_.reset(nullptr);
// std::string sn2 = device_->getDeviceInfo()->serialNumber(); device_.reset();
// if (sn1 == sn2) { }
// RCLCPP_ERROR(logger_, "The device with SN %s was disconnected!", sn1.c_str()); }
// ob_camera_node_.reset(nullptr); } catch (const ob::Error &e) {
// device_.reset(); RCLCPP_ERROR_STREAM(logger_, e.getMessage());
// } }
// }
// } catch (const ob::Error &e) {
// RCLCPP_ERROR_STREAM(logger_, e.getMessage());
// }
} }
void OBCameraNodeFactory::printDeviceInfo(const std::shared_ptr<ob::DeviceInfo> &device_info) { void OBCameraNodeFactory::printDeviceInfo(const std::shared_ptr<ob::DeviceInfo> &device_info) {
@@ -139,6 +137,7 @@ void OBCameraNodeFactory::getDevice(const std::shared_ptr<ob::DeviceList> &list)
if (dev != nullptr) { if (dev != nullptr) {
device_ = dev; device_ = dev;
printDeviceInfo(device_->getDeviceInfo()); printDeviceInfo(device_->getDeviceInfo());
updateDeviceInfo();
} }
} }
} else { } else {
@@ -147,10 +146,18 @@ void OBCameraNodeFactory::getDevice(const std::shared_ptr<ob::DeviceList> &list)
RCLCPP_ERROR_STREAM(logger_, "can not found device with SN " << serial_number_); RCLCPP_ERROR_STREAM(logger_, "can not found device with SN " << serial_number_);
} else { } else {
printDeviceInfo(device_->getDeviceInfo()); printDeviceInfo(device_->getDeviceInfo());
updateDeviceInfo();
} }
} }
} }
void OBCameraNodeFactory::updateDeviceInfo() {
if (device_ == nullptr) {
return;
}
device_info_ = device_->getDeviceInfo();
}
void OBCameraNodeFactory::startDevice() { void OBCameraNodeFactory::startDevice() {
if (ob_camera_node_) { if (ob_camera_node_) {
ob_camera_node_.reset(); ob_camera_node_.reset();