mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-04 12:07:46 +08:00
70 lines
2.2 KiB
C++
70 lines
2.2 KiB
C++
/*******************************************************************************
|
|
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*******************************************************************************/
|
|
#include "orbbec_camera/utils.h"
|
|
#include "orbbec_camera/synced_imu_publisher.h"
|
|
#include <rclcpp/rclcpp.hpp>
|
|
|
|
namespace orbbec_camera {
|
|
SyncedImuPublisher::SyncedImuPublisher(
|
|
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher, size_t queue_size)
|
|
: imu_publisher_(imu_publisher), queue_size_(queue_size) {}
|
|
|
|
SyncedImuPublisher::~SyncedImuPublisher() { publishPendingMessages(); }
|
|
|
|
void SyncedImuPublisher::publish(const sensor_msgs::msg::Imu &imu_msg) {
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
auto sub_num = imu_publisher_->get_subscription_count();
|
|
if (sub_num == 0 || !is_enabled_) {
|
|
return;
|
|
}
|
|
if (is_paused_) {
|
|
while (queue_.size() >= queue_size_) {
|
|
queue_.pop();
|
|
}
|
|
queue_.push(imu_msg);
|
|
} else {
|
|
imu_publisher_->publish(imu_msg);
|
|
}
|
|
}
|
|
|
|
void SyncedImuPublisher::pause() {
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
is_paused_ = true;
|
|
}
|
|
|
|
void SyncedImuPublisher::resume() {
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
is_paused_ = false;
|
|
publishPendingMessages();
|
|
}
|
|
|
|
void SyncedImuPublisher::setQueueSize(size_t queue_size) {
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
queue_size_ = queue_size;
|
|
}
|
|
|
|
void SyncedImuPublisher::enable(bool enable) { is_enabled_ = enable; }
|
|
|
|
void SyncedImuPublisher::publishPendingMessages() {
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
while (!queue_.empty()) {
|
|
imu_publisher_->publish(queue_.front());
|
|
queue_.pop();
|
|
}
|
|
}
|
|
|
|
} // namespace orbbec_camera
|