Files
DriverLIdar/src/json_mini.hpp
QUYVN 59880871b0 Fix Family B angle decode + add LR-1FMI model
parse_family_b() dùng sai hệ số góc 0.25°/LSB; theo spec Olei chính hãng
(Olei.LidarSensor/LidarDataBlock.GetAngleDegrees) AngleRaw là 0.01°/LSB.
Sai 25× khiến điểm bị gán nhầm góc → một phòng bị bôi thành vòng tròn trên
RViz. Đã verify với thiết bị thật OLELR-1FMI: sau khi sửa ra 2400 điểm/vòng,
0–359.9°, đúng hình học môi trường.

- Đổi hệ số góc 0.25° → 0.01° trong parse_family_b().
- Bỏ qua block invalid (AngleRaw >= 0xFF00) theo spec.
- Dò ranh giới vòng quay PER-POINT thay vì per-packet (một gói có thể chứa
  >1 vòng), tránh gộp nhiều vòng vào một scan.
- Thêm model LR-1FMI (360°, 0.01°/LSB, ~2400 pts/rev) vào bảng model +
  kModelTable, đặt "1FMI" trước "1F" để khớp đúng chuỗi tên.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
2026-07-01 10:14:52 +07:00

239 lines
7.6 KiB
C++

// json_mini.hpp — minimal header-only JSON parse/serialize, just enough for
// flat-ish config objects (no comments, no streaming, no error recovery).
// Not a general-purpose JSON library — kept tiny on purpose.
#pragma once
#include <string>
#include <vector>
#include <utility>
#include <cstdlib>
#include <cstring>
#include <cctype>
#include <stdexcept>
namespace json {
enum class Type { Null, Bool, Number, String, Object, Array };
struct Value {
Type type = Type::Null;
bool b = false;
double num = 0;
std::string str;
std::vector<Value> arr;
std::vector<std::pair<std::string, Value>> obj;
static Value make_object() { Value v; v.type = Type::Object; return v; }
static Value make_string(std::string s) { Value v; v.type = Type::String; v.str = std::move(s); return v; }
static Value make_number(double n) { Value v; v.type = Type::Number; v.num = n; return v; }
static Value make_bool(bool x) { Value v; v.type = Type::Bool; v.b = x; return v; }
void set(const std::string& key, Value v) {
for (auto& kv : obj) if (kv.first == key) { kv.second = std::move(v); return; }
obj.emplace_back(key, std::move(v));
}
const Value* find(const std::string& key) const {
for (auto& kv : obj) if (kv.first == key) return &kv.second;
return nullptr;
}
std::string get_string(const std::string& key, const std::string& def = "") const {
const Value* v = find(key);
return (v && v->type == Type::String) ? v->str : def;
}
double get_number(const std::string& key, double def = 0) const {
const Value* v = find(key);
return (v && v->type == Type::Number) ? v->num : def;
}
bool get_bool(const std::string& key, bool def = false) const {
const Value* v = find(key);
return (v && v->type == Type::Bool) ? v->b : def;
}
std::string dump() const {
std::string out;
dump_to(out);
return out;
}
private:
static void escape_into(const std::string& s, std::string& out) {
out += '"';
for (char c : s) {
switch (c) {
case '"': out += "\\\""; break;
case '\\': out += "\\\\"; break;
case '\n': out += "\\n"; break;
default: out += c; break;
}
}
out += '"';
}
void dump_to(std::string& out) const {
switch (type) {
case Type::Null: out += "null"; break;
case Type::Bool: out += b ? "true" : "false"; break;
case Type::Number: {
if (num == static_cast<long long>(num)) out += std::to_string(static_cast<long long>(num));
else out += std::to_string(num);
break;
}
case Type::String: escape_into(str, out); break;
case Type::Array: {
out += '[';
for (size_t i = 0; i < arr.size(); ++i) {
if (i) out += ',';
arr[i].dump_to(out);
}
out += ']';
break;
}
case Type::Object: {
out += '{';
for (size_t i = 0; i < obj.size(); ++i) {
if (i) out += ',';
escape_into(obj[i].first, out);
out += ':';
obj[i].second.dump_to(out);
}
out += '}';
break;
}
}
}
};
// ── Parser ──────────────────────────────────────────────────────────────
class ParseError : public std::runtime_error {
public:
explicit ParseError(const std::string& what) : std::runtime_error(what) {}
};
namespace detail {
class Parser {
public:
explicit Parser(const std::string& s) : s_(s) {}
Value parse() {
skip_ws();
Value v = parse_value();
skip_ws();
return v;
}
private:
const std::string& s_;
size_t pos_ = 0;
char peek() const {
if (pos_ >= s_.size()) throw ParseError("unexpected end of JSON");
return s_[pos_];
}
char next() { return s_[pos_++]; }
void skip_ws() { while (pos_ < s_.size() && std::isspace(static_cast<unsigned char>(s_[pos_]))) ++pos_; }
void expect(char c) {
if (pos_ >= s_.size() || s_[pos_] != c)
throw ParseError(std::string("expected '") + c + "'");
++pos_;
}
bool starts_with(const char* lit) {
size_t n = std::strlen(lit);
if (s_.compare(pos_, n, lit) == 0) { pos_ += n; return true; }
return false;
}
Value parse_value() {
skip_ws();
char c = peek();
if (c == '{') return parse_object();
if (c == '[') return parse_array();
if (c == '"') return Value::make_string(parse_string());
if (starts_with("true")) return Value::make_bool(true);
if (starts_with("false")) return Value::make_bool(false);
if (starts_with("null")) { Value v; v.type = Type::Null; return v; }
return parse_number();
}
Value parse_object() {
Value v = Value::make_object();
expect('{');
skip_ws();
if (peek() == '}') { ++pos_; return v; }
while (true) {
skip_ws();
std::string key = parse_string();
skip_ws();
expect(':');
Value val = parse_value();
v.obj.emplace_back(std::move(key), std::move(val));
skip_ws();
char c = next();
if (c == ',') continue;
if (c == '}') break;
throw ParseError("expected ',' or '}' in object");
}
return v;
}
Value parse_array() {
Value v; v.type = Type::Array;
expect('[');
skip_ws();
if (peek() == ']') { ++pos_; return v; }
while (true) {
v.arr.push_back(parse_value());
skip_ws();
char c = next();
if (c == ',') continue;
if (c == ']') break;
throw ParseError("expected ',' or ']' in array");
}
return v;
}
std::string parse_string() {
expect('"');
std::string out;
while (true) {
char c = next();
if (c == '"') break;
if (c == '\\') {
char e = next();
switch (e) {
case 'n': out += '\n'; break;
case 't': out += '\t'; break;
case '"': out += '"'; break;
case '\\': out += '\\'; break;
case '/': out += '/'; break;
default: out += e; break;
}
} else {
out += c;
}
}
return out;
}
Value parse_number() {
size_t start = pos_;
if (pos_ < s_.size() && (s_[pos_] == '-' || s_[pos_] == '+')) ++pos_;
while (pos_ < s_.size() &&
(std::isdigit(static_cast<unsigned char>(s_[pos_])) || s_[pos_] == '.' ||
s_[pos_] == 'e' || s_[pos_] == 'E' || s_[pos_] == '-' || s_[pos_] == '+'))
++pos_;
if (pos_ == start) throw ParseError("invalid number");
return Value::make_number(std::strtod(s_.substr(start, pos_ - start).c_str(), nullptr));
}
};
} // namespace detail
inline Value parse(const std::string& s) {
detail::Parser p(s);
return p.parse();
}
} // namespace json