请用c++写一段ros程序，读取深度相机的深度图像和rgb图像，两者的话题名分别为/camera3/depth/image_raw和/camera3/rgb/image_raw，读取到的深度图存入/home/hudi/my_pic/town2/depth/video1，rgb图片存入/home/hudi/my_pic/town2/rgb/video1路径。此外还需要保存小车的实时imu值，话题名字为/imu，并将每一帧的yaw值存入/home/hudi/my_pic/town2/depth/yaw.txt

好的，这是一个简单的ROS程序，可以读取深度相机的深度图像和rgb图像，保存到指定路径，并将每一帧的yaw值保存到txt文件中。

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <fstream>

using namespace std;
using namespace cv;

const string path_depth = "/home/hudi/my_pic/town2/depth/video1";
const string path_rgb = "/home/hudi/my_pic/town2/rgb/video1";
const string path_yaw = "/home/hudi/my_pic/town2/depth/yaw.txt";

ofstream outfile;

void depth_callback(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    Mat depth_image = cv_ptr->image;
    imwrite(path_depth, depth_image);
}

void rgb_callback(const sensor_msgs::ImageConstPtr& msg)
{
    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;
    }
    Mat rgb_image = cv_ptr->image;
    imwrite(path_rgb, rgb_image);
}

void imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
    double yaw = tf::getYaw(msg->orientation);
    outfile << yaw << endl;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "depth_rgb_imu_node");
    ros::NodeHandle nh;

    outfile.open(path_yaw.c_str(), ios::out);
    if (!outfile.is_open())
    {
        ROS_ERROR("Failed to open %s", path_yaw.c_str());
        return -1;
    }

    ros::Subscriber depth_sub = nh.subscribe("/camera3/depth/image_raw", 1, depth_callback);
    ros::Subscriber rgb_sub = nh.subscribe("/camera3/rgb/image_raw", 1, rgb_callback);
    ros::Subscriber imu_sub = nh.subscribe("/imu", 1, imu_callback);

    ros::spin();

    outfile.close();

    return 0;
}

程序的思路很简单，就是订阅深度相机的深度图像和rgb图像，保存到指定路径；同时订阅IMU话题，获取每一帧的yaw值并保存到txt文件中。