-
Notifications
You must be signed in to change notification settings - Fork 15
Expand file tree
/
Copy pathsave_data.cpp
More file actions
125 lines (109 loc) · 3.85 KB
/
Copy pathsave_data.cpp
File metadata and controls
125 lines (109 loc) · 3.85 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
#include "save_data.h"
#include "hardware_config/mandeye.h"
#include "save_laz.h"
#include <chrono>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
namespace mandeye
{
std::pair<std::string, std::optional<LazStats>> savePointcloudData(LidarPointsBufferPtr buffer, const std::string& directory, int chunk)
{
using namespace std::chrono_literals;
char lidarName[256];
const auto start = std::chrono::steady_clock::now();
snprintf(lidarName, 256, "lidar%04d.laz", chunk);
std::filesystem::path lidarFilePath = std::filesystem::path(directory) / std::filesystem::path(lidarName);
std::cout << "Savig lidar buffer of size " << buffer->size() << " to " << lidarFilePath << std::endl;
auto saveStatus = saveLaz(lidarFilePath.string(), buffer);
system("sync");
const auto end = std::chrono::steady_clock::now();
const std::chrono::duration<float> elapsed_seconds = end - start;
if(saveStatus)
{
saveStatus->m_saveDurationSec2 = elapsed_seconds.count();
hardware::OnSavedLaz(lidarFilePath);
}
else
{
std::cout << "Error saving laz file " << lidarFilePath << std::endl;
}
return {lidarFilePath.string(), saveStatus};
}
void saveLidarList(const std::unordered_map<uint32_t, std::string>& lidars, const std::string& directory, int chunk)
{
using namespace std::chrono_literals;
char lidarName[256];
snprintf(lidarName, 256, "lidar%04d.sn", chunk);
std::filesystem::path lidarFilePath = std::filesystem::path(directory) / std::filesystem::path(lidarName);
std::cout << "Savig lidar list of size " << lidars.size() << " to " << lidarFilePath << std::endl;
std::ofstream lidarStream(lidarFilePath);
for(const auto& [id, sn] : lidars)
{
lidarStream << id << " " << sn << "\n";
}
system("sync");
return;
}
void saveImuData(LidarIMUBufferPtr buffer, const std::string& directory, int chunk)
{
using namespace std::chrono_literals;
char lidarName[256];
snprintf(lidarName, 256, "imu%04d.csv", chunk);
std::filesystem::path lidarFilePath = std::filesystem::path(directory) / std::filesystem::path(lidarName);
std::cout << "Savig imu buffer of size " << buffer->size() << " to " << lidarFilePath << std::endl;
std::ofstream lidarStream(lidarFilePath.c_str());
lidarStream << "timestamp gyroX gyroY gyroZ accX accY accZ imuId timestampUnix\n";
std::stringstream ss;
for(const auto& p : *buffer)
{
if(p.timestamp > 0)
{
ss << p.timestamp << " " << p.gyro_x << " " << p.gyro_y << " " << p.gyro_z << " " << p.acc_x << " " << p.acc_y << " " << p.acc_z << " "
<< p.laser_id << " " << p.epoch_time << "\n";
}
}
lidarStream << ss.rdbuf();
lidarStream.close();
system("sync");
return;
}
void saveGnssData(std::deque<std::string>& buffer, const std::string& directory, int chunk)
{
using namespace std::chrono_literals;
char lidarName[256];
snprintf(lidarName, 256, "gnss%04d.gnss", chunk);
std::filesystem::path lidarFilePath = std::filesystem::path(directory) / std::filesystem::path(lidarName);
std::cout << "Savig gnss buffer of size " << buffer.size() << " to " << lidarFilePath << std::endl;
std::ofstream lidarStream(lidarFilePath.c_str());
std::stringstream ss;
for(const auto& p : buffer)
{
ss << p;
}
lidarStream << ss.rdbuf();
lidarStream.close();
system("sync");
return;
}
void saveGnssRawData(std::deque<std::string>& buffer, const std::string& directory, int chunk)
{
using namespace std::chrono_literals;
char lidarName[256];
snprintf(lidarName, 256, "gnss%04d.nmea", chunk);
std::filesystem::path lidarFilePath = std::filesystem::path(directory) / std::filesystem::path(lidarName);
std::cout << "Savig gnss raw buffer of size " << buffer.size() << " to " << lidarFilePath << std::endl;
std::ofstream lidarStream(lidarFilePath.c_str());
std::stringstream ss;
for(const auto& p : buffer)
{
ss << p;
}
lidarStream << ss.rdbuf();
lidarStream.close();
system("sync");
return;
}
} // namespace mandeye