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

remove multiple thread publish point cloud

Browse Source
This commit is contained in:
Joe Dong
2022-07-04 11:21:53 +08:00
parent a0abf05467
commit 56dffb3870
6 changed files with 4 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/CMakeLists.txt
-1
View File
@@ -51,7 +51,6 @@ add_library(${PROJECT_NAME} SHARED
src/ob_camera_node_factory.cpp
src/ob_camera_node.cpp
src/ros_param_backend.cpp
src/ob_point_cloud_publisher.cpp
src/ros_service.cpp
src/utils.cpp
)
orbbec_camera/include/orbbec_camera/ob_camera_node.h
-3
View File
@@ -48,8 +48,6 @@
#include "orbbec_camera/constants.h"
#include "orbbec_camera/dynamic_params.h"
#include "orbbec_camera/ob_point_cloud_publisher.h"
#define STREAM_NAME(sip) \
(static_cast<std::ostringstream&&>(std::ostringstream() \
<< _stream_name[sip.first] \
@@ -307,6 +305,5 @@ class OBCameraNode {
std::shared_ptr<std::thread> tf_thread_;
std::condition_variable tf_cv_;
double tf_publish_rate_ = 10.0;
std::unique_ptr<OBPointCloudPublisher> ob_point_cloud_publisher_;
};
} // namespace orbbec_camera
orbbec_camera/include/orbbec_camera/ob_point_cloud_publisher.h
-42
View File
@@ -1,42 +0,0 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <thread>
#include <condition_variable>
#include <queue>
#include <thread>
#include <atomic>
namespace orbbec_camera {
class OBPointCloudPublisher {
public:
explicit OBPointCloudPublisher(rclcpp::Node* node, size_t max_filter_size = 2);
~OBPointCloudPublisher();
void pushPointCloud(sensor_msgs::msg::PointCloud2&& point_cloud2);
void pushColorPointCloud(sensor_msgs::msg::PointCloud2&& point_cloud2);
void publishPointCloud();
void publishColorPointCloud();
private:
rclcpp::Node* node_;
rclcpp::Logger logger_;
std::atomic_bool is_alive_{false};
size_t max_filter_size_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_publisher_;
std::queue<sensor_msgs::msg::PointCloud2> point_cloud_q_;
std::mutex point_cloud_q_lock_;
std::condition_variable point_cloud_cv_;
std::thread point_cloud_thread_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr color_point_cloud_publisher_;
std::queue<sensor_msgs::msg::PointCloud2> color_point_cloud_q_;
std::mutex color_point_cloud_q_lock_;
std::condition_variable color_point_cloud_cv_;
std::thread color_point_cloud_thread_;
};
} // namespace orbbec_camera
orbbec_camera/params/ob_camera_params.yaml
+2 -2
View File
@@ -1,7 +1,7 @@
/**:
ros__parameters:
color_width: 1280
color_height: 720
color_width: 1920
color_height: 1080
color_fps: 30.0
color_frame_id: "color_frame"
color_optical_frame_id: "color_optical_frame"
orbbec_camera/src/ob_camera_node.cpp
+2 -5
View File
@@ -247,7 +247,6 @@ void OBCameraNode::setupPublishers() {
}
extrinsics_publisher_ = node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"extrinsic/depth_to_color", rclcpp::QoS{1}.transient_local());
ob_point_cloud_publisher_ = std::make_unique<OBPointCloudPublisher>(node_);
}
void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
@@ -311,8 +310,7 @@ void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_se
point_cloud_msg_.width = valid_count;
point_cloud_msg_.height = 1;
modifier.resize(valid_count);
ob_point_cloud_publisher_->pushColorPointCloud(std::move(point_cloud_msg_));
// depth_point_cloud_publisher_->publish(point_cloud_msg_);
depth_point_cloud_publisher_->publish(point_cloud_msg_);
}
void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
@@ -373,8 +371,7 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
point_cloud_msg_.width = valid_count;
point_cloud_msg_.height = 1;
modifier.resize(valid_count);
ob_point_cloud_publisher_->pushColorPointCloud(std::move(point_cloud_msg_));
// point_cloud_publisher_->publish(point_cloud_msg_);
point_cloud_publisher_->publish(point_cloud_msg_);
}
void OBCameraNode::frameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
orbbec_camera/src/ob_point_cloud_publisher.cpp
-71
View File
@@ -1,71 +0,0 @@
#include "orbbec_camera/ob_point_cloud_publisher.h"
namespace orbbec_camera {
using namespace std::chrono_literals;
OBPointCloudPublisher::OBPointCloudPublisher(rclcpp::Node* node, size_t max_filter_size)
: node_(node), logger_(node->get_logger()), max_filter_size_(max_filter_size) {
is_alive_.store(true);
using sensor_msgs::msg::PointCloud2;
color_point_cloud_publisher_ = node_->create_publisher<PointCloud2>(
"depth/color/points", rclcpp::QoS{1}.best_effort().keep_last(1));
point_cloud_publisher_ = node_->create_publisher<PointCloud2>(
"depth/points", rclcpp::QoS{1}.best_effort().keep_last(1));
point_cloud_thread_ = std::thread([this]() { publishPointCloud(); });
color_point_cloud_thread_ = std::thread([this]() { publishColorPointCloud(); });
}
OBPointCloudPublisher::~OBPointCloudPublisher() {
is_alive_.store(false);
if (point_cloud_thread_.joinable()) {
point_cloud_thread_.join();
}
if (color_point_cloud_thread_.joinable()) {
color_point_cloud_thread_.join();
}
}
void OBPointCloudPublisher::pushPointCloud(sensor_msgs::msg::PointCloud2&& point_cloud2) {
std::lock_guard<decltype(point_cloud_q_lock_)> lock(point_cloud_q_lock_);
while (point_cloud_q_.size() > max_filter_size_) {
point_cloud_q_.pop();
}
point_cloud_q_.push(point_cloud2);
point_cloud_cv_.notify_one();
}
void OBPointCloudPublisher::pushColorPointCloud(sensor_msgs::msg::PointCloud2&& point_cloud2) {
std::lock_guard<decltype(color_point_cloud_q_lock_)> lock(color_point_cloud_q_lock_);
while (color_point_cloud_q_.size() > max_filter_size_) {
color_point_cloud_q_.pop();
}
color_point_cloud_q_.push(point_cloud2);
color_point_cloud_cv_.notify_one();
}
void OBPointCloudPublisher::publishPointCloud() {
while (is_alive_) {
std::unique_lock<decltype(point_cloud_q_lock_)> lock(point_cloud_q_lock_);
point_cloud_cv_.wait_for(lock, 100ms, [this]() { return !point_cloud_q_.empty(); });
if (!point_cloud_q_.empty()) {
auto msg = point_cloud_q_.front();
point_cloud_q_.pop();
point_cloud_publisher_->publish(msg);
}
lock.unlock();
}
}
void OBPointCloudPublisher::publishColorPointCloud() {
while (is_alive_) {
std::unique_lock<decltype(color_point_cloud_q_lock_)> lock(color_point_cloud_q_lock_);
color_point_cloud_cv_.wait_for(lock, 100ms, [this]() { return !color_point_cloud_q_.empty(); });
if (!color_point_cloud_q_.empty()) {
auto msg = color_point_cloud_q_.front();
color_point_cloud_q_.pop();
color_point_cloud_publisher_->publish(msg);
}
lock.unlock();
}
}
} // namespace orbbec_camera