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

Enhance error handling by formatting error messages

Browse Source
This commit is contained in:
ob-yalian
2026-04-01 20:52:46 +08:00
parent ad55f90bcc
commit b82fc0c267
9 changed files with 108 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+27 -26
View File
@@ -288,7 +288,7 @@ void OBCameraNode::setupDevices() {
TRY_EXECUTE_BLOCK(device_->loadPreset(device_preset_.c_str()));
RCLCPP_INFO_STREAM(logger_, "Device preset " << device_->getCurrentPresetName() << " loaded");
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset: " << e.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset: " << e.what());
} catch (...) {
@@ -1569,7 +1569,7 @@ void OBCameraNode::setupProfiles() {
<< imu_rate_[stream_index]);
} catch (const ob::Error &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to setup << " << stream_name_[stream_index]
<< " profile: " << e.getMessage());
<< " profile: " << orbbec_camera::formatObErrorWithStatus(e));
enable_stream_[stream_index] = false;
stream_profile_[stream_index] = nullptr;
}
@@ -1653,7 +1653,7 @@ void OBCameraNode::startStreams() {
onNewFrameSetCallback(frame_set);
});
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline: " << e.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
RCLCPP_INFO_STREAM(logger_, "try to disable ir stream and try again");
enable_stream_[INFRA0] = false;
setupPipelineConfig();
@@ -1760,7 +1760,7 @@ void OBCameraNode::startIMUSyncStream() {
<< fullGyroScaleRangeToString(gyro_range)
<< ",rate:" << sampleRateToString(gyro_rate));
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to start IMU sync stream: " << e.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Failed to start IMU sync stream: " << orbbec_camera::formatObErrorWithStatus(e));
imu_sync_output_start_ = false;
} catch (...) {
RCLCPP_ERROR_STREAM(
@@ -1831,7 +1831,7 @@ void OBCameraNode::stopStreams() {
}
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(
logger_, "Failed to disable interleave frame during shutdown: " << e.getMessage());
logger_, "Failed to disable interleave frame during shutdown: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (...) {
RCLCPP_WARN_STREAM(logger_, "Failed to disable interleave frame during shutdown");
}
@@ -1841,7 +1841,7 @@ void OBCameraNode::stopStreams() {
"Device or pipeline not available during stop - likely disconnected");
}
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << e.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline");
}
@@ -1858,7 +1858,7 @@ void OBCameraNode::stopIMU() {
try {
imuPipeline_->stop();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline: " << e.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline");
}
@@ -1872,7 +1872,7 @@ void OBCameraNode::stopIMU() {
sensors_[stream_index]->stop();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop " << stream_name_[stream_index]
<< " stream: " << e.getMessage());
<< " stream: " << orbbec_camera::formatObErrorWithStatus(e));
}
imu_started_[stream_index] = false;
}
@@ -2356,8 +2356,8 @@ void OBCameraNode::setupTopics() {
setupPublishers();
setupDiagnosticUpdater();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.getMessage());
throw std::runtime_error(e.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << orbbec_camera::formatObErrorWithStatus(e));
throw std::runtime_error(orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.what());
throw std::runtime_error(e.what());
@@ -2417,8 +2417,9 @@ void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapp
status.add("Chip Bottom Temperature", temperature.chipBottomTemp);
status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Temperature is normal");
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to TemperatureUpdate1: " << e.getMessage());
status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, e.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Failed to TemperatureUpdate1: " << orbbec_camera::formatObErrorWithStatus(e));
status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR,
orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to TemperatureUpdate2: " << e.what());
status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, e.what());
@@ -2474,7 +2475,7 @@ void OBCameraNode::setupDiagnosticUpdater() {
diagnostic_cv_.notify_all();
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "Diagnostic update failed: "
<< e.getMessage() << " - Device may be disconnected");
<< orbbec_camera::formatObErrorWithStatus(e) << " - Device may be disconnected");
// Stop the diagnostic timer if device is having issues
try {
if (diagnostic_timer_) {
@@ -2491,7 +2492,7 @@ void OBCameraNode::setupDiagnosticUpdater() {
}
});
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup diagnostic updater: " << e.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Failed to setup diagnostic updater: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup diagnostic updater: " << e.what());
} catch (...) {
@@ -2726,7 +2727,7 @@ void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_
}
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, e.getMessage());
RCLCPP_ERROR_STREAM(logger_, orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, e.what());
} catch (...) {
@@ -3368,7 +3369,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
}
}
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.getMessage());
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what());
} catch (...) {
@@ -3483,7 +3484,7 @@ std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
try {
color_frame = filter->process(frame);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Format convert failed: " << e.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Format convert failed: " << orbbec_camera::formatObErrorWithStatus(e));
return nullptr;
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Format convert failed: " << e.what());
@@ -4140,7 +4141,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
ex = stream_profile->getExtrinsicTo(base_stream_profile);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get " << stream_name_[stream_index]
<< " extrinsic: " << e.getMessage());
<< " extrinsic: " << orbbec_camera::formatObErrorWithStatus(e));
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
}
@@ -4185,7 +4186,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[COLOR]);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to get " << frame_id << " extrinsic: " << e.getMessage());
"Failed to get " << frame_id << " extrinsic: " << orbbec_camera::formatObErrorWithStatus(e));
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
}
depth_to_other_extrinsics_[COLOR] = ex;
@@ -4201,7 +4202,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[INFRA0]);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to get " << frame_id << " extrinsic: " << e.getMessage());
"Failed to get " << frame_id << " extrinsic: " << orbbec_camera::formatObErrorWithStatus(e));
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
}
depth_to_other_extrinsics_[INFRA0] = ex;
@@ -4216,7 +4217,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[INFRA1]);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to get " << frame_id << " extrinsic: " << e.getMessage());
"Failed to get " << frame_id << " extrinsic: " << orbbec_camera::formatObErrorWithStatus(e));
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
}
depth_to_other_extrinsics_[INFRA1] = ex;
@@ -4231,7 +4232,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[INFRA2]);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to get " << frame_id << " extrinsic: " << e.getMessage());
"Failed to get " << frame_id << " extrinsic: " << orbbec_camera::formatObErrorWithStatus(e));
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
}
ex.trans[0] = -std::abs(ex.trans[0]);
@@ -4247,7 +4248,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[ACCEL]);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to get " << frame_id << " extrinsic: " << e.getMessage());
"Failed to get " << frame_id << " extrinsic: " << orbbec_camera::formatObErrorWithStatus(e));
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
}
depth_to_other_extrinsics_[ACCEL] = ex;
@@ -4262,7 +4263,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to get " << frame_id << " extrinsic: " << e.getMessage());
"Failed to get " << frame_id << " extrinsic: " << orbbec_camera::formatObErrorWithStatus(e));
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
}
depth_to_other_extrinsics_[GYRO] = ex;
@@ -4277,7 +4278,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
ex = stream_profile_[COLOR_LEFT]->getExtrinsicTo(stream_profile_[COLOR_RIGHT]);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to get " << frame_id << " extrinsic: " << e.getMessage());
"Failed to get " << frame_id << " extrinsic: " << orbbec_camera::formatObErrorWithStatus(e));
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
}
depth_to_other_extrinsics_[COLOR_LEFT] = ex;
@@ -4726,7 +4727,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
}
response->success = true;
} catch (const ob::Error &e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception &e) {
response->message = e.what();