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

Add device_status topic for state monitoring

Browse Source
This commit is contained in:
xiexun
2025-09-02 17:06:59 +08:00
parent 78c4d68fb1
commit 4c932d47a8
7 changed files with 186 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node_driver.cpp
+25
View File
@@ -231,6 +231,9 @@ void OBCameraNodeDriver::init() {
CHECK_NOTNULL(check_connect_timer_);
query_thread_ = std::make_shared<std::thread>([this]() { queryDevice(); });
reset_device_thread_ = std::make_shared<std::thread>([this]() { resetDevice(); });
device_status_timer_ = this->create_wall_timer(
std::chrono::milliseconds(1000 / device_status_interval_hz), [this]() { deviceStatusTimer(); });
}
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
@@ -442,6 +445,28 @@ void OBCameraNodeDriver::resetDevice() {
}
}
void OBCameraNodeDriver::deviceStatusTimer() {
if (ob_camera_node_ == nullptr) {
return;
}
orbbec_camera_msgs::msg::DeviceStatus status_msg;
status_msg.header.stamp = this->now();
status_msg.device_online = device_connected_.load();
ob_camera_node_->getColorStatus(status_msg);
ob_camera_node_->getDepthStatus(status_msg);
status_msg.connection_type = device_info_->getConnectionType();
auto camera_params = device_->getCalibrationCameraParamList();
bool calibration_from_device = (camera_params != nullptr && camera_params->count() > 0);
status_msg.calibration_from_device = calibration_from_device;
status_msg.calibration_from_launch_param = ob_camera_node_->isParamCalibrated();
// status_msg.calibration_from_user_file = ob_camera_node_->isWriteCustomerDataSuccess();
ob_camera_node_->publishDeviceStatus(status_msg);
}
void OBCameraNodeDriver::rebootDeviceCallback(
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response) {