c++ ros2 计算点云的法向量，并发布点云法向量话题。再写一个用于编译的CMakeLists.txt文件。

在C++中使用ROS 2处理点云并计算其法向量，你需要使用ROS 2的`rclcpp`库以及相关的点云处理库，如`pcl_ros`。下面是一个简单的示例，展示了如何读取点云数据、计算法向量并将其作为新的话题发布： ```cpp #include <rclcpp/rclcpp.hpp> #include <sensor_msgs/msg/point_cloud2.hpp> #include <pcl_ros/pcl_ros.h> class PointCloudNormalPublisher : public rclcpp::Node { public: PointCloudNormalPublisher() : node_("cloud_normal_publisher") { // 创建订阅者来接收点云数据 cloud_sub_ = node_.create_subscription<sensor_msgs::msg::PointCloud2>("input_cloud", 10, [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*msg, *cloud); // 使用PCL计算法向量 pcl::PolygonMesh mesh; pcl::fromROSMsg(msg, mesh); pcl:: marching_cubes<pcl::PointXYZ, pcl::Normal> surface Estimation; surface_Estimation.setInputCloud(cloud); surface_Estimation.setDepthThreshold(0.01); // 设置深度阈值 surface_Estimation.reconstruct(mesh); // 将法向量转换为ROS消息并发布 std_msgs::msg::Float32MultiArray normal_msg; for (auto &face : mesh.faces) { for (int i = 0; i < face.vertex_indices.size(); ++i) { auto vertex = cloud->points[face.vertex_indices[i]]; normal_msg.data.push_back(vertex.normal.x); normal_msg.data.push_back(vertex.normal.y); normal_msg.data.push_back(vertex.normal.z); } } point_cloud_normals_pub_.publish(normal_msg); }); // 创建发布者来发布法向量 point_cloud_normals_pub_ = node_.create_publisher<std_msgs::msg::Float32MultiArray>("output_normals", 10); } private: rclcpp::Node::SharedPtr node_; rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_; rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr point_cloud_normals_pub_; }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = PointCloudNormalPublisher::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; } ``` 对于`CMakeLists.txt`文件，你需要包含必要的库链接和编译选项，如下所示： ```cmake cmake_minimum_required(VERSION 3.5) project(point_cloud_normal_publisher) find_package(rclcpp REQUIRED) find_package(pcl REQUIRED) add_executable(point_cloud_normal_publisher src/main.cpp) target_link_libraries(point_cloud_normal_publisher ${rclcpp_LIBRARIES} pcl_common pcl_io pcl_features) # 配置ROS节点的启动信息 include_directories(${rclcpp_INCLUDE_DIRS}) include_directories(${pcl_INCLUDE_DIRS}) # 生成ROS消息头文件（如果需要） execute_process(COMMAND roscpp_generate_messages DEPENDENCIES std_msgs OUTPUT FILE RosMessages_generated) add_dependencies(point_cloud_normal_publisher RosMessages_generated) ``` 这个`CMakeLists.txt`将生成一个可执行文件`point_cloud_normal_publisher`，并在编译过程中链接所需的ROS和PCL库。