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

fixed HUB hot plug bug

Browse Source
This commit is contained in:
Joe Dong
2023-07-13 17:15:48 +08:00
parent b7673beb62
commit 66774bac3e
3 changed files with 38 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
+6
View File
@@ -72,6 +72,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
void syncTime();
void resetDevice();
private:
std::string config_path_;
std::unique_ptr<ob::Context> ctx_ = nullptr;
@@ -92,5 +94,9 @@ class OBCameraNodeDriver : public rclcpp::Node {
int num_devices_connected_ = 0;
rclcpp::TimerBase::SharedPtr check_connect_timer_ = nullptr;
std::shared_ptr<std::thread> sync_time_thread_ = nullptr;
std::shared_ptr<std::thread> reset_device_thread_ = nullptr;
std::mutex reset_device_mutex_;
std::condition_variable reset_device_cond_;
std::atomic_bool reset_device_flag_{false};
};
} // namespace orbbec_camera
orbbec_camera/launch/multi_camera.launch.xml
+4 -4
View File
@@ -6,21 +6,21 @@
<!-- stereo_s_u3, astrapro, astra -->
<arg name="camera1_prefix" default="01"/>
<arg name="camera2_prefix" default="02"/>
<arg name="camera1_serila_number" default="AY3A131007R"/>
<arg name="camera2_serila_number" default="AY3JB20003L"/>
<arg name="camera1_usb_port" default="2-3.3"/>
<arg name="camera2_usb_port" default="1-4.4"/>
<arg name="device_num" default="2"/>
<node name="camera" pkg="orbbec_camera" exec="ob_cleanup_shm_node" output="screen"/>
<group>
<include file="$(find-pkg-share orbbec_camera)/launch/$(var 3d_sensor).launch.xml">
<arg name="camera_name" value="camera_$(var camera1_prefix)"/>
<arg name="serial_number" value="$(var camera1_serila_number)"/>
<arg name="usb_port" value="$(var camera1_usb_port)"/>
<arg name="device_num" value="$(var device_num)"/>
</include>
</group>
<group>
<include file="$(find-pkg-share orbbec_camera)/launch/$(var 3d_sensor).launch.xml">
<arg name="camera_name" value="camera_$(var camera2_prefix)"/>
<arg name="serial_number" value="$(var camera2_serila_number)"/>
<arg name="usb_port" value="$(var camera2_usb_port)"/>
<arg name="device_num" value="$(var device_num)"/>
</include>
</group>
orbbec_camera/src/ob_camera_node_driver.cpp
+28 -8
View File
@@ -49,6 +49,10 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
if (query_thread_ && query_thread_->joinable()) {
query_thread_->join();
}
if (reset_device_thread_ && reset_device_thread_->joinable()) {
reset_device_cond_.notify_all();
reset_device_thread_->join();
}
}
void OBCameraNodeDriver::init() {
@@ -71,7 +75,7 @@ void OBCameraNodeDriver::init() {
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_);
reset_device_thread_ = std::make_shared<std::thread>([this]() { resetDevice(); });
}
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
@@ -105,11 +109,10 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
std::scoped_lock<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM(logger_, "device with " << uid << " disconnected");
if (uid == device_unique_id_) {
ob_camera_node_.reset();
device_.reset();
device_connected_ = false;
std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
reset_device_flag_ = true;
reset_device_cond_.notify_all();
current_device_disconnected = true;
device_unique_id_.clear();
break;
}
}
@@ -175,6 +178,21 @@ void OBCameraNodeDriver::syncTime() {
}
}
void OBCameraNodeDriver::resetDevice() {
while (is_alive_ && rclcpp::ok()) {
std::unique_lock<decltype(reset_device_mutex_)> lock(reset_device_mutex_);
reset_device_cond_.wait(lock,
[this]() { return !is_alive_ || !rclcpp::ok() || reset_device_flag_; });
if (!is_alive_ || !rclcpp::ok()) {
break;
}
ob_camera_node_.reset();
device_.reset();
device_connected_ = false;
device_unique_id_.clear();
reset_device_flag_ = false;
}
}
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);
@@ -316,15 +334,17 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
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");
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 1000,
"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);
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 1000, "Device usb port: " << uid);
if (port_id == usb_port) {
RCLCPP_INFO_STREAM(logger_, "Device usb port <<" << uid << " matched");
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 1000,
"Device usb port " << uid << " matched");
return list->getDevice(i);
}
}