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

chore: Add check when init device failed

Browse Source
This commit is contained in:
Joe Dong
2024-08-29 15:17:42 +08:00
parent e912829d4f
commit 082f7b2966
2 changed files with 46 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+12 -1
View File
@@ -608,7 +608,14 @@ void OBCameraNode::setupProfiles() {
CHECK_NOTNULL(profiles.get());
CHECK(profiles->count() > 0);
for (size_t i = 0; i < profiles->count(); i++) {
auto profile = profiles->getProfile(i)->as<ob::VideoStreamProfile>();
auto base_profile = profiles->getProfile(i);
if (base_profile == nullptr) {
throw std::runtime_error("Failed to get profile " + std::to_string(i));
}
auto profile = base_profile->as<ob::VideoStreamProfile>();
if (profile == nullptr) {
throw std::runtime_error("Failed cast profile to VideoStreamProfile");
}
RCLCPP_DEBUG_STREAM(
logger_, "Sensor profile: "
<< "stream_type: " << magic_enum::enum_name(profile->type())
@@ -640,6 +647,7 @@ void OBCameraNode::setupProfiles() {
"configuration and try again. The current process will now exit.");
RCLCPP_INFO_STREAM(logger_, "Available profiles:");
printSensorProfiles(sensor);
RCLCPP_ERROR(logger_, "Because can not set this stream, so exit.");
exit(-1);
}
@@ -1091,10 +1099,13 @@ void OBCameraNode::setupTopics() {
setupDiagnosticUpdater();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.getMessage());
throw std::runtime_error(e.getMessage());
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.what());
throw std::runtime_error(e.what());
} catch (...) {
RCLCPP_ERROR(logger_, "Failed to setup topics");
throw std::runtime_error("Failed to setup topics");
}
}
orbbec_camera/src/ob_camera_node_driver.cpp
+34 -4
View File
@@ -363,12 +363,42 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
if (ob_camera_node_) {
ob_camera_node_.reset();
}
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
node_options_.use_intra_process_comms());
int retry_count = 0;
const int max_retries = 3;
bool initialized = false;
device_info_ = device_->getDeviceInfo();
RCLCPP_INFO_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
while (retry_count < max_retries && !initialized) {
try {
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
node_options_.use_intra_process_comms());
initialized = true;
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
<< retry_count + 1 << " of " << max_retries
<< "): " << e.getMessage());
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt " << retry_count + 1
<< " of " << max_retries
<< "): " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
<< retry_count + 1 << " of " << max_retries << ")");
}
retry_count++;
}
if (!initialized) {
RCLCPP_ERROR_STREAM(logger_,
"Device initialization failed after " << max_retries << " attempts.");
throw std::runtime_error("Device initialization failed after " + std::to_string(max_retries) +
" attempts.");
}
ob_camera_node_->startIMU();
ob_camera_node_->startStreams();
device_connected_ = true;
device_info_ = device_->getDeviceInfo();
serial_number_ = device_info_->serialNumber();
CHECK_NOTNULL(device_info_.get());
device_unique_id_ = device_info_->uid();
@@ -390,7 +420,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start_time_);
RCLCPP_INFO_STREAM(logger_, "Start device cost " << time_cost.count() << " ms");
}
} // namespace orbbec_camera
void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int net_device_port) {
if (net_device_ip.empty() || net_device_port == 0) {