114 lines
3.4 KiB
C++
114 lines
3.4 KiB
C++
#include "fgc/ScanGrid.h"
|
|
|
|
#include <fstream>
|
|
#include <sstream>
|
|
#include <stdexcept>
|
|
#include <string>
|
|
|
|
namespace fgc {
|
|
|
|
namespace {
|
|
|
|
std::string trim(const std::string& s) {
|
|
size_t b = s.find_first_not_of(" \t\r\n");
|
|
if (b == std::string::npos) return "";
|
|
size_t e = s.find_last_not_of(" \t\r\n");
|
|
return s.substr(b, e - b + 1);
|
|
}
|
|
|
|
std::vector<double> parseDoubleList(const std::string& csv) {
|
|
std::vector<double> out;
|
|
size_t start = 0;
|
|
while (start <= csv.size()) {
|
|
size_t comma = csv.find(',', start);
|
|
std::string tok =
|
|
trim(csv.substr(start, comma == std::string::npos ? std::string::npos : comma - start));
|
|
if (!tok.empty()) out.push_back(std::stod(tok));
|
|
if (comma == std::string::npos) break;
|
|
start = comma + 1;
|
|
}
|
|
return out;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
std::vector<ScanPoint> parseScanCsv(const std::string& text) {
|
|
std::vector<ScanPoint> pts;
|
|
std::istringstream in(text);
|
|
std::string line;
|
|
int lineno = 0;
|
|
while (std::getline(in, line)) {
|
|
++lineno;
|
|
std::string t = trim(line);
|
|
if (t.empty() || t[0] == '#') continue;
|
|
size_t comma = t.find(',');
|
|
if (comma == std::string::npos)
|
|
throw std::runtime_error("scan grid line " + std::to_string(lineno) +
|
|
": expected 'yaw_deg,pitch_deg', got '" + t + "'");
|
|
try {
|
|
ScanPoint p;
|
|
p.yaw_deg = std::stod(trim(t.substr(0, comma)));
|
|
p.pitch_deg = std::stod(trim(t.substr(comma + 1)));
|
|
pts.push_back(p);
|
|
} catch (const std::exception&) {
|
|
throw std::runtime_error("scan grid line " + std::to_string(lineno) +
|
|
": non-numeric coordinate in '" + t + "'");
|
|
}
|
|
}
|
|
return pts;
|
|
}
|
|
|
|
std::vector<ScanPoint> generateScanGrid(const ScanConfig& cfg) {
|
|
std::vector<ScanPoint> pts;
|
|
std::vector<double> pitches = parseDoubleList(cfg.pitch_levels);
|
|
if (pitches.empty()) pitches.push_back(0.0);
|
|
int n = cfg.yaw_intervals;
|
|
if (n < 1) n = 1;
|
|
// pitch-major, yaw inner (min -> max); the runtime cursor ping-pongs the whole list.
|
|
for (double pitch : pitches) {
|
|
for (int i = 0; i < n; ++i) {
|
|
double frac = n > 1 ? static_cast<double>(i) / (n - 1) : 0.0;
|
|
ScanPoint p;
|
|
p.yaw_deg = cfg.yaw_min_deg + frac * (cfg.yaw_max_deg - cfg.yaw_min_deg);
|
|
p.pitch_deg = pitch;
|
|
pts.push_back(p);
|
|
}
|
|
}
|
|
return pts;
|
|
}
|
|
|
|
ScanGrid ScanGrid::fromConfig(const ScanConfig& cfg) {
|
|
if (!cfg.grid_file.empty()) {
|
|
std::ifstream f(cfg.grid_file);
|
|
if (!f) throw std::runtime_error("cannot open scan grid file: " + cfg.grid_file);
|
|
std::stringstream buf;
|
|
buf << f.rdbuf();
|
|
auto pts = parseScanCsv(buf.str());
|
|
if (pts.empty())
|
|
throw std::runtime_error("scan grid file has no waypoints: " + cfg.grid_file);
|
|
return ScanGrid(std::move(pts));
|
|
}
|
|
return ScanGrid(generateScanGrid(cfg));
|
|
}
|
|
|
|
void ScanGrid::advance() {
|
|
if (points_.size() <= 1) return;
|
|
if (forward_) {
|
|
if (index_ + 1 < points_.size()) {
|
|
++index_;
|
|
} else {
|
|
forward_ = false;
|
|
--index_;
|
|
}
|
|
} else {
|
|
if (index_ > 0) {
|
|
--index_;
|
|
} else {
|
|
forward_ = true;
|
|
++index_;
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace fgc
|