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

use single thread publish point cloud

Browse Source
This commit is contained in:
Joe Dong
2022-06-30 11:39:33 +08:00
parent 306864d921
commit a0abf05467
6 changed files with 136 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/CMakeLists.txt
+1
View File
@@ -51,6 +51,7 @@ 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,6 +48,8 @@
#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] \
@@ -305,5 +307,6 @@ 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
@@ -0,0 +1,42 @@
#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
+5 -5
View File
@@ -1,7 +1,7 @@
/**:
ros__parameters:
color_width: 2048
color_height: 1536
color_width: 1280
color_height: 720
color_fps: 30.0
color_frame_id: "color_frame"
color_optical_frame_id: "color_optical_frame"
@@ -12,8 +12,8 @@
ir_frame_id: "ir_frame"
ir_optical_frame_id: "ir_optical_frame"
enable_ir: true
depth_width: 1280
depth_height: 1024
depth_width: 640
depth_height: 480
depth_fps: 30.0
depth_frame_id: "depth_frame"
depth_optical_frame_id: "depth_optical_frame"
@@ -22,7 +22,7 @@
tf_publish_rate: 10.0
wait_for_device_timeout: 120.0
reconnect_timeout: 6.0
d2c_mode: "none"
d2c_mode: "hw"
serial_number: ""
camera_link_frame_id: "camera_link"
ob_log_level: "none"
orbbec_camera/src/ob_camera_node.cpp
+14 -11
View File
@@ -247,6 +247,7 @@ 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) {
@@ -310,7 +311,8 @@ 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);
depth_point_cloud_publisher_->publish(point_cloud_msg_);
ob_point_cloud_publisher_->pushColorPointCloud(std::move(point_cloud_msg_));
// depth_point_cloud_publisher_->publish(point_cloud_msg_);
}
void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
@@ -327,7 +329,7 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
auto* points = (OBColorPoint*)frame->data();
CHECK_NOTNULL(points);
sensor_msgs::PointCloud2Modifier modifier(point_cloud_msg_);
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(point_size);
point_cloud_msg_.width = color_frame->width();
point_cloud_msg_.height = color_frame->height();
@@ -345,15 +347,15 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(point_cloud_msg_, "b");
size_t valid_count = 0;
for (size_t point_idx = 0; point_idx < point_size; point_idx++, points++) {
bool valid_pixel(points->z > 0);
for (size_t point_idx = 0; point_idx < point_size; point_idx += 1) {
bool valid_pixel((points + point_idx)->z > 0);
if (valid_pixel) {
*iter_x = static_cast<float>(points->x / 1000.0);
*iter_y = -static_cast<float>(points->y / 1000.0);
*iter_z = static_cast<float>(points->z / 1000.0);
*iter_r = static_cast<uint8_t>(points->r);
*iter_g = static_cast<uint8_t>(points->g);
*iter_b = static_cast<uint8_t>(points->b);
*iter_x = static_cast<float>((points + point_idx)->x / 1000.0);
*iter_y = -static_cast<float>((points + point_idx)->y / 1000.0);
*iter_z = static_cast<float>((points + point_idx)->z / 1000.0);
*iter_r = static_cast<uint8_t>((points + point_idx)->r);
*iter_g = static_cast<uint8_t>((points + point_idx)->g);
*iter_b = static_cast<uint8_t>((points + point_idx)->b);
++iter_x;
++iter_y;
@@ -371,7 +373,8 @@ 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);
point_cloud_publisher_->publish(point_cloud_msg_);
ob_point_cloud_publisher_->pushColorPointCloud(std::move(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
@@ -0,0 +1,71 @@
#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