From 900e4011499121a1482b5f71197eb1ee0721c6e6 Mon Sep 17 00:00:00 2001 From: jj <957713278@qq.com> Date: Tue, 10 Jun 2025 22:39:05 +0800 Subject: [PATCH] Add support sphere point ouput --- .../include/orbbec_camera/ob_lidar_node.h | 10 +- orbbec_camera/launch/lidar.launch.py | 2 +- orbbec_camera/src/ob_lidar_node.cpp | 102 +++++++++++++----- 3 files changed, 77 insertions(+), 37 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_lidar_node.h b/orbbec_camera/include/orbbec_camera/ob_lidar_node.h index fb30b865..746c60fe 100644 --- a/orbbec_camera/include/orbbec_camera/ob_lidar_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_lidar_node.h @@ -166,6 +166,9 @@ class OBLidarNode { void onNewFrameSetCallback(std::shared_ptr frame_set); + std::vector spherePointToPoint(OBLiDARSpherePoint* sphere_point, + uint32_t point_count); + void publishScan(std::shared_ptr frame_set); void publishPointCloud(std::shared_ptr frame_set); @@ -191,7 +194,6 @@ class OBLidarNode { rclcpp::Node* node_ = nullptr; std::shared_ptr device_ = nullptr; std::shared_ptr parameters_ = nullptr; - std::condition_variable color_frame_queue_cv_; rclcpp::Logger logger_; std::atomic_bool is_running_{false}; std::unique_ptr pipeline_ = nullptr; @@ -212,12 +214,6 @@ class OBLidarNode { std::atomic_bool is_camera_node_initialized_{false}; std::mutex device_lock_; - std::shared_ptr colorFrameThread_ = nullptr; - uint8_t* rgb_buffer_ = nullptr; - uint8_t* rgb_point_cloud_buffer_ = nullptr; - float* xy_table_data_ = nullptr; - float* depth_xy_table_data_ = nullptr; - uint8_t* depth_point_cloud_buffer_ = nullptr; bool publish_tf_ = false; bool tf_published_ = false; std::shared_ptr static_tf_broadcaster_ = nullptr; diff --git a/orbbec_camera/launch/lidar.launch.py b/orbbec_camera/launch/lidar.launch.py index c2af1e47..bdb3138b 100644 --- a/orbbec_camera/launch/lidar.launch.py +++ b/orbbec_camera/launch/lidar.launch.py @@ -60,7 +60,7 @@ def generate_launch_description(): DeclareLaunchArgument('frame_id', default_value='scan'), DeclareLaunchArgument('tf_publish_rate', default_value='0.0'), DeclareLaunchArgument('lidar_format', default_value='ANY'), - DeclareLaunchArgument('lidar_rate', default_value='30'), + DeclareLaunchArgument('lidar_rate', default_value='15'), DeclareLaunchArgument('min_angle', default_value='-135.0'), DeclareLaunchArgument('max_angle', default_value='135.0'), DeclareLaunchArgument('min_range', default_value='0.05'), diff --git a/orbbec_camera/src/ob_lidar_node.cpp b/orbbec_camera/src/ob_lidar_node.cpp index ee97d7b6..3db4db78 100644 --- a/orbbec_camera/src/ob_lidar_node.cpp +++ b/orbbec_camera/src/ob_lidar_node.cpp @@ -90,29 +90,8 @@ void OBLidarNode::clean() noexcept { tf_thread_->join(); } RCLCPP_WARN_STREAM(logger_, "Stop color frame thread"); - if (colorFrameThread_ && colorFrameThread_->joinable()) { - color_frame_queue_cv_.notify_all(); - colorFrameThread_->join(); - } - RCLCPP_WARN_STREAM(logger_, "stop streams"); stopStreams(); - // stopIMU(); - delete[] rgb_buffer_; - rgb_buffer_ = nullptr; - - delete[] rgb_point_cloud_buffer_; - rgb_point_cloud_buffer_ = nullptr; - - delete[] xy_table_data_; - xy_table_data_ = nullptr; - - delete[] depth_xy_table_data_; - depth_xy_table_data_ = nullptr; - - delete[] depth_point_cloud_buffer_; - depth_point_cloud_buffer_ = nullptr; - RCLCPP_WARN_STREAM(logger_, "Destroy ~OBLidarNode DONE"); } @@ -144,8 +123,9 @@ void OBLidarNode::getParameters() { param_name = stream_name_[stream_index] + "_rate"; setAndGetNodeParameter(rate_int_[stream_index], param_name, 0); rate_[stream_index] = OBScanRateFromInt(rate_int_[stream_index]); - RCLCPP_INFO_STREAM(logger_, "rate_ " << magic_enum::enum_name(rate_[LIDAR]) << " format_" - << magic_enum::enum_name(format_[LIDAR])); + RCLCPP_INFO_STREAM( + logger_, "Input rate: " << magic_enum::enum_name(rate_[LIDAR]) + << " Input format:" << magic_enum::enum_name(format_[LIDAR])); param_name = stream_name_[stream_index] + "_frame_id"; std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame"; setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id); @@ -297,8 +277,6 @@ void OBLidarNode::setupPublishers() { if (use_intra_process_) { point_cloud_qos_profile = rmw_qos_profile_default; } - RCLCPP_INFO_STREAM(logger_, "rate_ " << magic_enum::enum_name(rate_[LIDAR]) << "format_" - << magic_enum::enum_name(format_[LIDAR])); if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN) { scan_pub_ = node_->create_publisher( "scan/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile), @@ -449,7 +427,7 @@ void OBLidarNode::publishPointCloud(std::shared_ptr frame_set) { } auto lidar_frame = frame_set->getFrame(OB_FRAME_LIDAR_POINTS); auto *point_data = reinterpret_cast(lidar_frame->getData()); - auto point_count = lidar_frame->getDataSize() / sizeof(OBLiDARScanPoint); + auto point_count = lidar_frame->getDataSize() / sizeof(OBLiDARPoint); auto frame_timestamp = getFrameTimestampUs(lidar_frame); auto timestamp = fromUsToROSTime(frame_timestamp); auto point_cloud_msg = std::make_unique(); @@ -487,6 +465,46 @@ void OBLidarNode::publishPointCloud(std::shared_ptr frame_set) { void OBLidarNode::publishSpherePointCloud(std::shared_ptr frame_set) { (void)frame_set; + if (frame_set == nullptr) { + return; + } + auto lidar_frame = frame_set->getFrame(OB_FRAME_LIDAR_POINTS); + auto *point_data = reinterpret_cast(lidar_frame->getData()); + auto point_count = lidar_frame->getDataSize() / sizeof(OBLiDARSpherePoint); + auto result_point = spherePointToPoint(point_data, point_count); + auto frame_timestamp = getFrameTimestampUs(lidar_frame); + auto timestamp = fromUsToROSTime(frame_timestamp); + auto point_cloud_msg = std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg); + modifier.setPointCloud2Fields(5, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + sensor_msgs::msg::PointField::FLOAT32, "z", 1, + sensor_msgs::msg::PointField::FLOAT32, "reflectivity", 1, + sensor_msgs::msg::PointField::UINT8, "tag", 1, + sensor_msgs::msg::PointField::UINT8); + modifier.resize(point_count); + point_cloud_msg->header.stamp = timestamp; + point_cloud_msg->header.frame_id = frame_id_[LIDAR]; + point_cloud_msg->height = 1; + point_cloud_msg->width = point_count; + point_cloud_msg->is_dense = true; + point_cloud_msg->is_bigendian = false; + point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step; + point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step); + sensor_msgs::PointCloud2Iterator iter_x(*point_cloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*point_cloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*point_cloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_reflectivity(*point_cloud_msg, "reflectivity"); + sensor_msgs::PointCloud2Iterator iter_tag(*point_cloud_msg, "tag"); + for (size_t i = 0; i < point_count; + ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_reflectivity, ++iter_tag) { + *iter_x = static_cast(result_point[i].x / 1000.0); + *iter_y = static_cast(result_point[i].y / 1000.0); + *iter_z = static_cast(result_point[i].z / 1000.0); + *iter_reflectivity = result_point[i].reflectivity; + *iter_tag = result_point[i].tag; + } + *point_cloud_msg = filterPointCloud(*point_cloud_msg); + point_cloud_pub_->publish(std::move(point_cloud_msg)); } uint64_t OBLidarNode::getFrameTimestampUs(const std::shared_ptr &frame) { @@ -503,6 +521,32 @@ uint64_t OBLidarNode::getFrameTimestampUs(const std::shared_ptr &fram } } +std::vector OBLidarNode::spherePointToPoint(OBLiDARSpherePoint *sphere_point, + uint32_t point_count) { + std::vector point_cloud; + point_cloud.resize(point_count); + OBLiDARPoint *point_data = point_cloud.data(); + for (uint32_t i = 0; i < point_count; ++i) { + double theta_rad = sphere_point->theta * M_PI / 180.0f; // to unit rad + double phi_rad = sphere_point->phi * M_PI / 180.0f; // to unit rad + auto distance = sphere_point->distance / 1000; // mm to m + auto x = static_cast(distance * cos(theta_rad) * cos(phi_rad)); + auto y = static_cast(distance * sin(theta_rad) * cos(phi_rad)); + auto z = static_cast(distance * sin(phi_rad)); + if (std::isfinite(x) && std::isfinite(y) && std::isfinite(y)) { + // point: convert to opengl point + point_data->x = x; + point_data->y = y; + point_data->z = z; + point_data->reflectivity = sphere_point->reflectivity; + point_data->tag = sphere_point->tag; + ++point_data; + } + ++sphere_point; + } + return point_cloud; +} + void OBLidarNode::filterScan(sensor_msgs::msg::LaserScan &scan) { double current_angle = scan.angle_min; double max_angle = deg2rad(max_angle_); @@ -567,11 +611,12 @@ sensor_msgs::msg::PointCloud2 OBLidarNode::filterPointCloud( sensor_msgs::PointCloud2Iterator iter_x(point_cloud, "x"); sensor_msgs::PointCloud2Iterator iter_y(point_cloud, "y"); sensor_msgs::PointCloud2Iterator iter_z(point_cloud, "z"); - sensor_msgs::PointCloud2Iterator iter_intensity(point_cloud, "intensity"); + sensor_msgs::PointCloud2Iterator iter_reflectivity(point_cloud, "reflectivity"); + sensor_msgs::PointCloud2Iterator iter_tag(point_cloud, "tag"); // Process each point for (size_t i = 0; i < point_cloud.height * point_cloud.width; - ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity) { + ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_reflectivity, ++iter_tag) { float x = *iter_x; float y = *iter_y; float z = *iter_z; @@ -605,7 +650,6 @@ sensor_msgs::msg::PointCloud2 OBLidarNode::filterPointCloud( return filtered_point_cloud; } - void OBLidarNode::publishStaticTransforms() { if (!publish_tf_) { return;