i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-13 03:30:18 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix point cloud filter

Browse Source
This commit is contained in:
Joe Dong
2022-06-24 12:09:19 +08:00
parent edd2acc081
commit f97fbfe683
3 changed files with 24 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/CMakeLists.txt
+9 -3
View File
@@ -3,9 +3,11 @@ project(orbbec_camera)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_C_STANDARD 11)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
set(CMAKE_BUILD_TYPE "Debug")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC -O3")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fPIC -g")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -O3")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fPIC -g")
set(CMAKE_BUILD_TYPE "Release")
if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic -Werror)
@@ -33,6 +35,9 @@ find_package(tf2_eigen REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_sensor_msgs REQUIRED)
find_package(OpenMP REQUIRED)
find_package(Threads REQUIRED)
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
@@ -93,6 +98,7 @@ target_link_libraries(${PROJECT_NAME}
Eigen3::Eigen
glog::glog
magic_enum::magic_enum
OpenMP::OpenMP_CXX
)
install(TARGETS ${PROJECT_NAME}
orbbec_camera/include/orbbec_camera/constants.h
+3
View File
@@ -14,6 +14,9 @@
#include <string>
#include <cstdlib>
#include <omp.h>
#define THREAD_NUM 4
#define OB_ROS_MAJOR_VERSION 1
#define OB_ROS_MINOR_VERSION 0
orbbec_camera/src/ob_camera_node.cpp
+12 -3
View File
@@ -40,7 +40,7 @@ OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> devic
if (device_pid == FEMTO_PID || device_pid == FEMTO_LIVE_PID || device_pid == FEMTO_OW_PID) {
format_[COLOR] = OB_FORMAT_I420;
} else if (device_pid == ASTRA_PLUS_PID || device_pid == ASTRA_PLUS_S_PID) {
format_[COLOR] = OB_FORMAT_YUYV;
format_[COLOR] = OB_FORMAT_MJPG;
} else {
// default RGB888
format_[COLOR] = OB_FORMAT_RGB888;
@@ -226,8 +226,8 @@ void OBCameraNode::setupPublishers() {
static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
using PointCloud2 = sensor_msgs::msg::PointCloud2;
using CameraInfo = sensor_msgs::msg::CameraInfo;
point_cloud_publisher_ =
node_->create_publisher<PointCloud2>("depth/points", rclcpp::QoS{1}.best_effort());
point_cloud_publisher_ = node_->create_publisher<PointCloud2>(
"depth/points", rclcpp::QoS{1}.best_effort().keep_last(1));
for (const auto& stream_index : IMAGE_STREAMS) {
std::string name = stream_name_[stream_index.first];
std::string topic = name + "/image_raw";
@@ -297,6 +297,10 @@ void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_se
auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp());
point_cloud_msg_.header.stamp = timestamp;
point_cloud_msg_.header.frame_id = optical_frame_id_[DEPTH];
point_cloud_msg_.is_dense = true;
point_cloud_msg_.width = valid_count;
point_cloud_msg_.height = 1;
modifier.resize(valid_count);
point_cloud_publisher_->publish(point_cloud_msg_);
}
@@ -330,6 +334,7 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(point_cloud_msg_, "g");
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);
if (valid_pixel) {
@@ -352,6 +357,10 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp());
point_cloud_msg_.header.stamp = timestamp;
point_cloud_msg_.header.frame_id = optical_frame_id_[COLOR];
point_cloud_msg_.is_dense = true;
point_cloud_msg_.width = valid_count;
point_cloud_msg_.height = 1;
modifier.resize(valid_count);
point_cloud_publisher_->publish(point_cloud_msg_);
}