10 #include <boost/filesystem.hpp>
11 #include <boost/range/iterator_range.hpp>
16 #include <tf2_msgs/TFMessage.h>
18 namespace fs = boost::filesystem;
25 std::string timeToString(
const ros::Time& cur_ros_t)
27 std::time_t cur_t = cur_ros_t.
sec;
29 ss << std::put_time(std::localtime(&cur_t),
"%Y-%m-%d-%H-%M-%S");
33 std::string getNewFilename(
const std::string& old)
35 for(
int i = 2; i < 10; ++i)
38 if(!fs::exists(tst_filename))
45 std::vector<fs::path> getBagFilesInCurrentFolder(
const std::string& filename)
47 std::vector<fs::path> bagFiles;
48 auto path = fs::absolute(fs::path(filename)).parent_path();
50 for(
auto& entry : boost::make_iterator_range(fs::directory_iterator(path), {}))
52 auto filePath = entry.path();
53 if(fs::extension(filePath) ==
".bag")
55 bagFiles.emplace_back(filePath);
62 std::vector<fs::path> sortFilesByTime(
const std::vector<fs::path>& files)
64 std::multimap<std::time_t,fs::path> timeFiles;
65 for(
auto & entry : files)
67 timeFiles.emplace(fs::last_write_time(entry),entry);
70 std::vector<fs::path> timeSortedFiles;
71 if(!timeFiles.empty())
72 std::transform(timeFiles.begin(), timeFiles.end(), std::back_inserter(timeSortedFiles), [](
auto &kv){ return kv.second;});
74 return timeSortedFiles;
77 std::uint64_t getTotalSizeInBytes(
const std::vector<fs::path>& files)
79 std::uint64_t totalSizeInBytes = 0;
80 for(
auto& entry : files)
81 totalSizeInBytes += fs::file_size(entry);
83 return totalSizeInBytes;
89 , m_filename{filename}
90 , m_namingMode{namingMode}
91 , m_splitSizeInBytes{splitSizeInBytes}
92 , m_deleteOldAtInBytes{deleteOldAtInBytes}
100 m_tf_header = boost::make_shared<std::map<std::string, std::string>>();
102 auto& connectionHeader = *m_tf_header;
103 connectionHeader[
"topic"] =
"/tf_static";
106 connectionHeader[
"type"] =
"tf2_msgs/TFMessage";
108 connectionHeader[
"latching"] =
"1";
113 if(m_deleteOldAtInBytes != 0)
137 std::unique_lock<std::mutex> lock(
m_mutex);
154 if(msg && msg->topic ==
"/tf_static")
156 auto shifter = msg->message.getMessage();
157 auto tf_msg = shifter->instantiate<tf2_msgs::TFMessage>();
160 for(
auto& transformMsg : tf_msg->transforms)
177 std::unique_lock<std::mutex> lock(
m_mutex);
184 std::string filename;
198 if(fs::exists(filename))
200 auto replacement = getNewFilename(filename);
201 if(replacement.empty())
203 ROSFMT_ERROR(
"Could not find a bag file name to write to (template '{}')", filename);
210 filename = replacement;
217 ROSFMT_INFO(
"Opening bag file: {}", filename.c_str());
225 auto tf_msg = boost::make_shared<tf2_msgs::TFMessage>();
226 std::vector<std::string> frames;
229 for(
auto& frame : frames)
236 tf_msg->transforms.push_back(std::move(transform));
251 std::unique_lock<std::mutex> lock(
m_mutex);
270 namespace fs = boost::filesystem;
272 auto path = fs::absolute(fs::path(
m_filename)).parent_path();
273 auto space = fs::space(path);
280 namespace fs = boost::filesystem;
282 using namespace std::literals;
286 std::vector<fs::path> bagFiles = getBagFilesInCurrentFolder(
m_filename);
295 std::vector<fs::path> bagFiles = getBagFilesInCurrentFolder(
m_filename);
297 std::vector<fs::path> timeSortedBagFiles = sortFilesByTime(bagFiles);
301 for(
auto& entry : timeSortedBagFiles)
304 if(fs::equivalent(entry, currentPath))
307 std::uint64_t bagSize = fs::file_size(entry);
309 ROSFMT_INFO(
"Bag directory requires too much space. Removing old bag file: {}", entry.c_str());
321 ROSFMT_WARN(
"I could not remove enough bag files to make the bag file directory small enough.");
338 std::unique_lock<std::mutex> lock{
m_mutex};