请用c++写一段ros程序，gazebo仿真环境下有一个realsense深度相机,要求读取深度相机的深度图像和rgb图像，两者的话题名分别为/camera3/depth/image_raw和/camera3/rgb/image_raw，深度图片的编码格式为32FC1，读取图片并分别显示

好的，这是一个简单的ROS程序，可以读取realsense深度相机的深度图像和rgb图像，并在rviz中分别显示。 ```cpp #include <ros/ros.h> #include <sensor_msgs/Image.h> #include <sensor_msgs/image_encodings.h> #include <cv_bridge/cv_bridge.h> #include <opencv2/highgui/highgui.hpp> // 定义全局变量，用于存储深度图像和RGB图像 cv::Mat depth_image; cv::Mat rgb_image; // 深度图像的回调函数 void depthCallback(const sensor_msgs::ImageConstPtr& msg) { // 将ROS消息转换为OpenCV图像 cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; } // 保存深度图像 depth_image = cv_ptr->image; } // RGB图像的回调函数 void rgbCallback(const sensor_msgs::ImageConstPtr& msg) { // 将ROS消息转换为OpenCV图像 cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; } // 保存RGB图像 rgb_image = cv_ptr->image; } int main(int argc, char** argv) { // 初始化ROS节点 ros::init(argc, argv, "realsense_reader"); ros::NodeHandle nh; // 订阅深度图像和RGB图像的话题 ros::Subscriber depth_sub = nh.subscribe("/camera3/depth/image_raw", 1, depthCallback); ros::Subscriber rgb_sub = nh.subscribe("/camera3/rgb/image_raw", 1, rgbCallback); // 创建OpenCV窗口 cv::namedWindow("Depth Image"); cv::namedWindow("RGB Image"); // 循环读取深度图像和RGB图像，并在OpenCV窗口中显示 while (ros::ok()) { // 显示深度图像 if (!depth_image.empty()) { cv::imshow("Depth Image", depth_image); } // 显示RGB图像 if (!rgb_image.empty()) { cv::imshow("RGB Image", rgb_image); } // 等待按键，退出程序 char key = cv::waitKey(10); if (key == 27) // ESC键 { break; } // 处理ROS回调函数 ros::spinOnce(); } // 销毁OpenCV窗口 cv::destroyAllWindows(); return 0; } ``` 需要注意的是，该程序中使用了OpenCV库来显示深度图像和RGB图像，因此需要在CMakeLists.txt文件中添加OpenCV的依赖项。完整的CMakeLists.txt文件如下所示： ```cmake cmake_minimum_required(VERSION 2.8.3) project(realsense_reader) find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs cv_bridge image_transport ) find_package(OpenCV REQUIRED) catkin_package( CATKIN_DEPENDS roscpp sensor_msgs cv_bridge image_transport ) include_directories( ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) add_executable(${PROJECT_NAME} src/realsense_reader.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ) install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) ``` 另外，需要在.launch文件中启动Gazebo仿真环境和realsense深度相机节点，并将/camera3/depth/image_raw和/camera3/rgb/image_raw话题分别映射到程序中定义的回调函数中。完整的.launch文件如下所示： ```xml <launch> <!-- 启动Gazebo仿真环境 --> <include file="$(find gazebo_ros)/launch/empty_world.launch"> <arg name="world_name" value="$(find realsense_gazebo)/worlds/realsense_world.world"/> </include> <!-- 启动realsense深度相机节点 --> <node name="realsense_node" pkg="realsense2_camera" type="realsense2_camera_node"> <param name="serial_no" value="123456789"/> <param name="enable_depth" value="true"/> <param name="enable_color" value="true"/> <param name="depth_height" value="480"/> <param name="depth_width" value="640"/> <param name="color_height" value="480"/> <param name="color_width" value="640"/> <param name="depth_fps" value="30"/> <param name="color_fps" value="30"/> </node> <!-- 映射/camera3/depth/image_raw话题 --> <node name="depth_image" pkg="image_transport" type="republish"> <remap from="/image_raw" to="/camera3/depth/image_raw"/> <remap from="/compressed" to="/camera3/depth/compressed"/> <remap from="/compressedDepth" to="/camera3/depth/compressedDepth"/> <remap from="/theora" to="/camera3/depth/theora"/> </node> <!-- 映射/camera3/rgb/image_raw话题 --> <node name="rgb_image" pkg="image_transport" type="republish"> <remap from="/image_raw" to="/camera3/rgb/image_raw"/> <remap from="/compressed" to="/camera3/rgb/compressed"/> <remap from="/compressedDepth" to="/camera3/rgb/compressedDepth"/> <remap from="/theora" to="/camera3/rgb/theora"/> </node> <!-- 启动realsense_reader节点 --> <node name="realsense_reader" pkg="realsense_reader" type="realsense_reader"/> </launch> ``` 启动.launch文件后，即可在rviz中看到深度图像和RGB图像的显示。