[人工智能]ROS之读取rosbag并保存到新rosbag

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <ros/ros.h>

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

int main()
{
    rosbag::Bag bag, bw;
    bag.open("/home/lwd/data/cti_all_bag/2022/apriltag.bag", rosbag::bagmode::Read);
    bw.open("/home/lwd/data/cti_all_bag/2022/res.bag", rosbag::bagmode::Write);
    // 要用的topic
    std::vector<std::string> topics;
    topics.push_back(std::string("/scan"));
    topics.push_back(std::string("/color/image_raw"));
    // 创建view
    rosbag::View view(bag, rosbag::TopicQuery(topics));
    // 遍历
    rosbag::View::iterator it = view.begin();
    for (; it != view.end(); ++it)
    {
        auto m = *it;
        // 处理scan
        sensor_msgs::LaserScan::ConstPtr input = m.instantiate<sensor_msgs::LaserScan>();
        if (input != NULL)
        {
            int sz = input->ranges.size();
            sensor_msgs::LaserScan scan;
            scan.header.frame_id = "laser_merge";
            scan.ranges.resize(sz);
            scan.angle_increment = 0.00436332309619;
            scan.angle_max = 3.14159274101;
            scan.angle_min = -3.14159274101;
            scan.range_min = 0.0799999982119;
            scan.range_max = 40.0;
            scan.time_increment = 0.0;
            for (int i = 0; i < sz; ++i)
            {
                if (input->ranges[i] > 0.8 && input->ranges[i] < 0.844)
                {
                    scan.ranges[i] = input->ranges[i];
                }
            }
            //ROS_INFO("%d",);
            bw.write("/scan", input->header.stamp, scan);
        }
        // 处理图像
        sensor_msgs::Image::ConstPtr im = m.instantiate<sensor_msgs::Image>();
        if (im != NULL)
            bw.write("/color/image_raw", im->header.stamp, *im);
    }

    bag.close();
    bw.close();

    return 0;
}