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>
This commit is contained in:
238
src/json_mini.hpp
Normal file
238
src/json_mini.hpp
Normal file
@@ -0,0 +1,238 @@
|
||||
// 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
|
||||
Reference in New Issue
Block a user