#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;
}
ROS之读取rosbag并保存到新rosbag
最新推荐文章于 2023-07-12 23:56:54 发布