libs/kdl/src/eu/kdl/kdl.h
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | #pragma once | ||
| 2 | |||
| 3 | #include <functional> | ||
| 4 | #include <map> | ||
| 5 | #include <optional> | ||
| 6 | #include <stdexcept> | ||
| 7 | #include <string> | ||
| 8 | #include <string_view> | ||
| 9 | #include <type_traits> | ||
| 10 | #include <variant> | ||
| 11 | #include <vector> | ||
| 12 | |||
| 13 | // forward-declare the types used from the C headers as not to pollute the global namespace | ||
| 14 | typedef struct kdl_str kdl_str; | ||
| 15 | typedef struct kdl_number kdl_number; | ||
| 16 | typedef struct kdl_value kdl_value; | ||
| 17 | typedef struct _kdl_parser kdl_parser; | ||
| 18 | |||
| 19 | namespace eu::kdl { | ||
| 20 | |||
| 21 | enum class KdlVersion { | ||
| 22 | Kdl_1, | ||
| 23 | Kdl_2, | ||
| 24 | Any | ||
| 25 | }; | ||
| 26 | |||
| 27 | template <typename T> concept _arithmetic = std::is_arithmetic_v<T>; | ||
| 28 | |||
| 29 | class TypeError : public std::exception { | ||
| 30 | const char* m_msg; | ||
| 31 | |||
| 32 | public: | ||
| 33 | TypeError() : m_msg{"kdlpp type error"} {} | ||
| 34 | ✗ | TypeError(const char* msg) : m_msg{msg} {} | |
| 35 | ✗ | const char* what() const noexcept { return m_msg; } | |
| 36 | }; | ||
| 37 | |||
| 38 | // Exception thrown on regular KDL parsing errors | ||
| 39 | class ParseError : public std::exception { | ||
| 40 | std::string m_msg; | ||
| 41 | |||
| 42 | public: | ||
| 43 | ParseError(kdl_str const& msg); | ||
| 44 | ✗ | ParseError(std::string msg) : m_msg{std::move(msg)} {} | |
| 45 | ✗ | const char* what() const noexcept { return m_msg.c_str(); } | |
| 46 | }; | ||
| 47 | |||
| 48 | // Exception thrown on KDL emitter errors (should never occur) | ||
| 49 | class EmitterError : public std::exception { | ||
| 50 | std::string m_msg; | ||
| 51 | |||
| 52 | public: | ||
| 53 | ✗ | EmitterError(std::string msg) : m_msg{std::move(msg)} {} | |
| 54 | ✗ | EmitterError() : EmitterError{"The KDL emitter encountered an error"} {} | |
| 55 | ✗ | const char* what() const noexcept { return m_msg.c_str(); } | |
| 56 | }; | ||
| 57 | |||
| 58 | // Ways in which a KDL number may be represented in C/C++ | ||
| 59 | enum NumberRepresentation { | ||
| 60 | Integer = 0, | ||
| 61 | Float, | ||
| 62 | String | ||
| 63 | }; | ||
| 64 | |||
| 65 | // A KDL number: could be a long long, a double, or a string | ||
| 66 | // Analogous to kdl_number | ||
| 67 | class Number { | ||
| 68 | std::variant<long long, double, std::string> m_value; | ||
| 69 | |||
| 70 | public: | ||
| 71 | Number() : m_value{0ll} {} | ||
| 72 | Number(long long n) : m_value{n} {} | ||
| 73 | Number(long n) : m_value{(long long)n} {} | ||
| 74 | Number(int n) : m_value{(long long)n} {} | ||
| 75 | Number(short n) : m_value{(long long)n} {} | ||
| 76 | Number(double n) : m_value{n} {} | ||
| 77 | Number(float n) : m_value{(double)n} {} | ||
| 78 | Number(const kdl_number& n); | ||
| 79 | |||
| 80 | ✗ | Number(Number const&) = default; | |
| 81 | ✗ | Number(Number&&) = default; | |
| 82 | Number& operator=(Number const&) = default; | ||
| 83 | ✗ | Number& operator=(Number&&) = default; | |
| 84 | |||
| 85 | bool operator==(const Number&) const = default; | ||
| 86 | bool operator!=(const Number&) const = default; | ||
| 87 | |||
| 88 | NumberRepresentation representation() const noexcept | ||
| 89 | { | ||
| 90 | return static_cast<NumberRepresentation>(m_value.index()); | ||
| 91 | } | ||
| 92 | |||
| 93 | // Cast the number to a fundamental arithmetic type (no bounds checking, | ||
| 94 | // no support for strings) | ||
| 95 | template <_arithmetic T> | ||
| 96 | ✗ | T as() const | |
| 97 | { | ||
| 98 | ✗ | if (std::holds_alternative<long long>(m_value)) { | |
| 99 | ✗ | return static_cast<T>(std::get<long long>(m_value)); | |
| 100 | ✗ | } else if (std::holds_alternative<double>(m_value)) { | |
| 101 | ✗ | return static_cast<T>(std::get<double>(m_value)); | |
| 102 | } else { | ||
| 103 | // string | ||
| 104 | ✗ | throw std::runtime_error("Number is stored as a string."); | |
| 105 | } | ||
| 106 | } | ||
| 107 | |||
| 108 | // Cast this C++ object to a libkdl C struct | ||
| 109 | // Note this object may hold a pointer to our string representation | ||
| 110 | explicit operator kdl_number() const; | ||
| 111 | }; | ||
| 112 | |||
| 113 | template <typename T> concept _into_number = requires(T t) { Number{t}; }; | ||
| 114 | |||
| 115 | // Mixin | ||
| 116 | class HasTypeAnnotation { | ||
| 117 | std::optional<std::string> m_type_annotation; | ||
| 118 | |||
| 119 | protected: | ||
| 120 | ✗ | HasTypeAnnotation() = default; | |
| 121 | ✗ | HasTypeAnnotation(std::string_view t) : m_type_annotation{t} {} | |
| 122 | |||
| 123 | public: | ||
| 124 | ✗ | const std::optional<std::string>& type_annotation() const { return m_type_annotation; } | |
| 125 | |||
| 126 | ✗ | void set_type_annotation(std::string_view type_annotation) | |
| 127 | { | ||
| 128 | ✗ | m_type_annotation = std::string{type_annotation}; | |
| 129 | ✗ | } | |
| 130 | |||
| 131 | void remove_type_annotation() { m_type_annotation.reset(); } | ||
| 132 | |||
| 133 | bool operator==(const HasTypeAnnotation&) const = default; | ||
| 134 | bool operator!=(const HasTypeAnnotation&) const = default; | ||
| 135 | }; | ||
| 136 | |||
| 137 | // KDL data types | ||
| 138 | enum class Type { | ||
| 139 | Null, | ||
| 140 | Bool, | ||
| 141 | Number, | ||
| 142 | String | ||
| 143 | }; | ||
| 144 | |||
| 145 | // A KDL value, possibly including a type annotation | ||
| 146 | // Analogous to kdl_value | ||
| 147 | class Value : public HasTypeAnnotation { | ||
| 148 | std::variant<std::monostate, bool, Number, std::string> m_value; | ||
| 149 | |||
| 150 | public: | ||
| 151 | ✗ | Value() = default; | |
| 152 | Value(bool b) : m_value{b} {} | ||
| 153 | Value(std::string_view s) : m_value{std::string{s}} {} | ||
| 154 | Value(std::string s) : m_value{std::move(s)} {} | ||
| 155 | Value(char const* s) : m_value{std::string{s}} {} | ||
| 156 | |||
| 157 | Value(Number n) : m_value{std::move(n)} {} | ||
| 158 | Value(_into_number auto n) : m_value{Number{n}} {} | ||
| 159 | |||
| 160 | Value(std::string_view type_annotation, bool b) : HasTypeAnnotation{type_annotation}, m_value{b} {} | ||
| 161 | Value(std::string_view type_annotation, std::string_view s) | ||
| 162 | : HasTypeAnnotation{type_annotation}, | ||
| 163 | m_value{std::string{s}} | ||
| 164 | { | ||
| 165 | } | ||
| 166 | Value(std::string_view type_annotation, std::string s) | ||
| 167 | : HasTypeAnnotation{type_annotation}, | ||
| 168 | m_value{std::move(s)} | ||
| 169 | { | ||
| 170 | } | ||
| 171 | Value(std::string_view type_annotation, Number n) | ||
| 172 | : HasTypeAnnotation{type_annotation}, | ||
| 173 | m_value{std::move(n)} | ||
| 174 | { | ||
| 175 | } | ||
| 176 | Value(std::string_view type_annotation, _into_number auto n) | ||
| 177 | : HasTypeAnnotation{type_annotation}, | ||
| 178 | m_value{std::move(n)} | ||
| 179 | { | ||
| 180 | } | ||
| 181 | |||
| 182 | Value(kdl_value const& val); | ||
| 183 | [[nodiscard]] static Value from_string(std::string_view s); | ||
| 184 | |||
| 185 | ✗ | Value(Value const&) = default; | |
| 186 | ✗ | Value(Value&&) = default; | |
| 187 | Value& operator=(Value const&) = default; | ||
| 188 | ✗ | Value& operator=(Value&&) = default; | |
| 189 | |||
| 190 | Value& operator=(bool b) | ||
| 191 | { | ||
| 192 | m_value = b; | ||
| 193 | return *this; | ||
| 194 | } | ||
| 195 | |||
| 196 | Value& operator=(std::string_view s) | ||
| 197 | { | ||
| 198 | m_value = std::string{s}; | ||
| 199 | return *this; | ||
| 200 | } | ||
| 201 | |||
| 202 | Value& operator=(std::string s) | ||
| 203 | { | ||
| 204 | m_value = std::move(s); | ||
| 205 | return *this; | ||
| 206 | } | ||
| 207 | |||
| 208 | Value& operator=(Number const& n) | ||
| 209 | { | ||
| 210 | m_value = n; | ||
| 211 | return *this; | ||
| 212 | } | ||
| 213 | |||
| 214 | Value& operator=(Number&& n) | ||
| 215 | { | ||
| 216 | m_value = std::move(n); | ||
| 217 | return *this; | ||
| 218 | } | ||
| 219 | |||
| 220 | Value& operator=(_into_number auto n) | ||
| 221 | { | ||
| 222 | m_value = Number{n}; | ||
| 223 | return *this; | ||
| 224 | } | ||
| 225 | |||
| 226 | bool operator==(const Value&) const = default; | ||
| 227 | bool operator!=(const Value&) const = default; | ||
| 228 | |||
| 229 | void set_to_null() { m_value = std::monostate{}; } | ||
| 230 | |||
| 231 | Type type() const noexcept { return static_cast<Type>(m_value.index()); } | ||
| 232 | |||
| 233 | ✗ | const Number& as_number() const | |
| 234 | { | ||
| 235 | ✗ | if (std::holds_alternative<Number>(m_value)) { | |
| 236 | ✗ | return std::get<Number>(m_value); | |
| 237 | } else { | ||
| 238 | ✗ | throw TypeError{"Value is not a number"}; | |
| 239 | } | ||
| 240 | } | ||
| 241 | |||
| 242 | ✗ | const std::string& as_string() const | |
| 243 | { | ||
| 244 | ✗ | if (std::holds_alternative<std::string>(m_value)) { | |
| 245 | ✗ | return std::get<std::string>(m_value); | |
| 246 | } else { | ||
| 247 | ✗ | throw TypeError{"Value is not a string"}; | |
| 248 | } | ||
| 249 | } | ||
| 250 | |||
| 251 | bool as_bool() const | ||
| 252 | { | ||
| 253 | if (std::holds_alternative<bool>(m_value)) { | ||
| 254 | return std::get<bool>(m_value); | ||
| 255 | } else { | ||
| 256 | throw TypeError{"Value is not a boolean"}; | ||
| 257 | } | ||
| 258 | } | ||
| 259 | |||
| 260 | bool is_null() const | ||
| 261 | { | ||
| 262 | return std::holds_alternative<std::monostate>(m_value); | ||
| 263 | } | ||
| 264 | |||
| 265 | explicit operator kdl_value() const; | ||
| 266 | }; | ||
| 267 | |||
| 268 | // A node with all its contents | ||
| 269 | class Node : public HasTypeAnnotation { | ||
| 270 | std::optional<std::string> m_type_annotation; | ||
| 271 | std::string m_name; | ||
| 272 | std::vector<Value> m_args; | ||
| 273 | std::map<std::string, Value, std::less<>> m_properties; | ||
| 274 | std::vector<Node> m_children; | ||
| 275 | |||
| 276 | public: | ||
| 277 | Node() = default; | ||
| 278 | Node(Node const&) = default; | ||
| 279 | ✗ | Node(Node&&) = default; | |
| 280 | ✗ | Node(std::string_view name) : m_name{name} {} | |
| 281 | ✗ | Node(std::string_view type_annotation, std::string_view name) | |
| 282 | ✗ | : HasTypeAnnotation{type_annotation}, | |
| 283 | ✗ | m_name{name} | |
| 284 | { | ||
| 285 | ✗ | } | |
| 286 | Node(std::string_view name, | ||
| 287 | std::vector<Value> args, | ||
| 288 | std::map<std::string, Value, std::less<>> properties, | ||
| 289 | std::vector<Node> children) | ||
| 290 | : m_name{name}, | ||
| 291 | m_args{std::move(args)}, | ||
| 292 | m_properties{std::move(properties)}, | ||
| 293 | m_children{std::move(children)} | ||
| 294 | { | ||
| 295 | } | ||
| 296 | Node(std::string_view type_annotation, | ||
| 297 | std::string_view name, | ||
| 298 | std::vector<Value> args, | ||
| 299 | std::map<std::string, Value, std::less<>> properties, | ||
| 300 | std::vector<Node> children) | ||
| 301 | : HasTypeAnnotation{type_annotation}, | ||
| 302 | m_name{name}, | ||
| 303 | m_args{std::move(args)}, | ||
| 304 | m_properties{std::move(properties)}, | ||
| 305 | m_children{std::move(children)} | ||
| 306 | { | ||
| 307 | } | ||
| 308 | |||
| 309 | Node& operator=(Node const&) = default; | ||
| 310 | Node& operator=(Node&&) = default; | ||
| 311 | |||
| 312 | ✗ | std::string const& name() const { return m_name; } | |
| 313 | void set_name(std::string_view name) { m_name = std::string{name}; } | ||
| 314 | |||
| 315 | ✗ | const std::vector<Value>& args() const { return m_args; } | |
| 316 | ✗ | std::vector<Value>& args() { return m_args; } | |
| 317 | ✗ | const std::map<std::string, Value, std::less<>>& properties() const { return m_properties; } | |
| 318 | ✗ | std::map<std::string, Value, std::less<>>& properties() { return m_properties; } | |
| 319 | ✗ | const std::vector<Node>& children() const { return m_children; } | |
| 320 | ✗ | std::vector<Node>& children() { return m_children; } | |
| 321 | }; | ||
| 322 | |||
| 323 | // A KDL document - consisting of several nodes. | ||
| 324 | class Document { | ||
| 325 | std::vector<Node> m_nodes; | ||
| 326 | |||
| 327 | public: | ||
| 328 | static Document read_from(kdl_parser* parser); | ||
| 329 | |||
| 330 | Document() = default; | ||
| 331 | Document(Document const&) = default; | ||
| 332 | ✗ | Document(Document&&) = default; | |
| 333 | Document(std::vector<Node> nodes) : m_nodes{std::move(nodes)} {} | ||
| 334 | Document(std::initializer_list<Node> nodes) : m_nodes{nodes} {} | ||
| 335 | |||
| 336 | Document& operator=(Document const&) = default; | ||
| 337 | Document& operator=(Document&&) = default; | ||
| 338 | |||
| 339 | const std::vector<Node>& nodes() const { return m_nodes; } | ||
| 340 | ✗ | std::vector<Node>& nodes() { return m_nodes; } | |
| 341 | |||
| 342 | ✗ | auto begin() const { return m_nodes.begin(); } | |
| 343 | auto begin() { return m_nodes.begin(); } | ||
| 344 | ✗ | auto end() const { return m_nodes.end(); } | |
| 345 | auto end() { return m_nodes.end(); } | ||
| 346 | |||
| 347 | std::string to_string() const; | ||
| 348 | std::string to_string(KdlVersion version) const; | ||
| 349 | }; | ||
| 350 | |||
| 351 | // Load a KDL document from string | ||
| 352 | Document parse(std::string_view kdl_text); | ||
| 353 | Document parse(std::string_view kdl_text, KdlVersion version); | ||
| 354 | |||
| 355 | } // namespace kdl | ||
| 356 |