在ROS2中将深度图数据转换为三维坐标的c++示例

很高兴为您回答这个问题。在ROS2中，将深度图数据转换为三维标可以使用以下C++示例代码：

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <geometry_msgs/msg/point_cloud2.hpp>
#include <image_geometry/pinhole_camera_model.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>

using namespace cv;

class DepthToPCLNode : public rclcpp::Node
{
public:
  DepthToPCLNode()
  : Node("depth_to_pcl_node") {
    depth_camera_info_sub_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
      "camera_info", rclcpp::SensorDataQoS(),
      std::bind(&DepthToPCLNode::depthCameraInfoCallback, this, std::placeholders::_1));

    depth_image_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
      "depth_image", rclcpp::SensorDataQoS(),
      std::bind(&DepthToPCLNode::depthImageCallback, this, std::placeholders::_1));

    point_cloud_pub_ = this->create_publisher<geometry_msgs::msg::PointCloud2>(
      "point_cloud", rclcpp::SensorDataQoS());
  }

private:
  void depthCameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg) {
    image_geometry::PinholeCameraModel camera_model;
    camera_model.fromCameraInfo(camera_info_msg);
    fx_ = camera_model.fx();
    fy_ = camera_model.fy();
    cx_ = camera_model.cx();
    cy_ = camera_model.cy();
    camera_info_received_ = true;
  }

  void depthImageCallback(const sensor_msgs::msg::Image::SharedPtr depth_image_msg) {
    if (!camera_info_received_) {
      return;
    }

    cv_bridge::CvImagePtr depth_image_ptr;
    try {
      depth_image_ptr = cv_bridge::toCvCopy(depth_image_msg, sensor_msgs::image_encodings::TYPE_32FC1);
    }
    catch (cv_bridge::Exception& e) {
      RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
      return;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());

    for (size_t v = 0; v < depth_image_ptr->image.rows; ++v) {
      for (size_t u = 0; u < depth_image_ptr->image.cols; ++u) {
        float depth = depth_image_ptr->image.at<float>(v, u);
        if (!std::isnan(depth) && depth > 0.0f) {
          pcl::PointXYZ point_cloud_point;
          point_cloud_point.x = (u - cx_) * depth / fx_;
          point_cloud_point.y = (v - cy_) * depth / fy_;
          point_cloud_point.z = depth;
          point_cloud->points.push_back(point_cloud_point);
        }
      }
    }

    geometry_msgs::msg::PointCloud2 point_cloud_msg;
    pcl::toROSMsg(*point_cloud, point_cloud_msg);
    point_cloud_msg.header = depth_image_msg->header;
    point_cloud_pub_->publish(point_cloud_msg);
  }

  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr depth_camera_info_sub_;
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr depth_image_sub_;
  rclcpp::Publisher<geometry_msgs::msg::PointCloud2>::SharedPtr point_cloud_pub_;
  bool camera_info_received_ = false;
  float fx_, fy_, cx_, cy_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<DepthToPCLNode>());
  rclcpp::shutdown();
  return 0;
}

这个示例代码包括ROS2中的所有必需头文件和订阅/发布相关的示例代码。当收到深度相机信息和深度图像时，它将创建一个点云并将其转换为ROS PointCloud2消息发布到`point cloud`主题。