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

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.cpp
+12
View File
@@ -91,6 +91,9 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
fps_counter_depth_->setLogLevel(log_level);
fps_counter_left_ir_->setLogLevel(log_level);
fps_counter_right_ir_->setLogLevel(log_level);
fps_delay_status_color_ = std::make_unique<FpsDelayStatus>();
fps_delay_status_depth_ = std::make_unique<FpsDelayStatus>();
}
template <class T>
@@ -2265,6 +2268,9 @@ void OBCameraNode::setupPublishers() {
std_msgs::msg::String msg;
msg.data = filter_status_.dump(2);
filter_status_pub_->publish(msg);
device_status_pub_ =
node_->create_publisher<orbbec_camera_msgs::msg::DeviceStatus>("device_status", extrinsics_qos);
}
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
@@ -3062,6 +3068,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
image_msg->header.frame_id = frame_id;
CHECK(image_publishers_.count(stream_index) > 0);
saveImageToFile(stream_index, image, *image_msg);
if (stream_index == COLOR) {
fps_delay_status_color_->tick();
}
else if (stream_index == DEPTH) {
fps_delay_status_depth_->tick();
}
image_publishers_[stream_index]->publish(std::move(image_msg));
if (stream_index == COLOR && enable_color_undistortion_ &&
color_undistortion_publisher_->get_subscription_count() > 0) {
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) {