From f129337204506668bc7d9fcc2e7b2cfacbaeb2b1 Mon Sep 17 00:00:00 2001 From: ahmad-ra Date: Sun, 3 Mar 2024 17:10:00 +0200 Subject: [PATCH 1/4] Add JSON<->ROS message serialization/deserialization. Add required tests. --- include/rosx_introspection/ros_parser.hpp | 414 +++++---- .../ros_utils/ros2_helpers.hpp | 6 +- src/ros_parser.cpp | 841 ++++++++++++++---- test/test_ros1.cpp | 110 +++ test/test_ros2.cpp | 112 +++ 5 files changed, 1150 insertions(+), 333 deletions(-) diff --git a/include/rosx_introspection/ros_parser.hpp b/include/rosx_introspection/ros_parser.hpp index a6c24b7..2dd3019 100644 --- a/include/rosx_introspection/ros_parser.hpp +++ b/include/rosx_introspection/ros_parser.hpp @@ -27,222 +27,278 @@ #include "rosx_introspection/stringtree_leaf.hpp" #include "rosx_introspection/deserializer.hpp" +#include "rapidjson/document.h" +#include "rapidjson/stringbuffer.h" +#include "rapidjson/writer.h" namespace RosMsgParser { -struct FlatMessage -{ - std::shared_ptr schema; - - /// List of all those parsed fields that can be represented by a - /// builtin value different from "string". - std::vector> value; + struct FlatMessage + { + std::shared_ptr schema; - /// List of all those parsed fields that can be represented by a "string". - std::vector> name; + /// List of all those parsed fields that can be represented by a + /// builtin value different from "string". + std::vector> value; - /// Store "blobs", i.e all those fields which are vectors of BYTES (AKA uint8_t), - /// where the vector size is greater than the argument [max_array_size]. - std::vector>> blob; + /// List of all those parsed fields that can be represented by a "string". + std::vector> name; - std::vector> blob_storage; -}; + /// Store "blobs", i.e all those fields which are vectors of BYTES (AKA uint8_t), + /// where the vector size is greater than the argument [max_array_size]. + std::vector>> blob; -class Parser -{ -public: - /** - * - * @param topic_name name of the topic to be used as node of the StringTree - * @param msg_type message type of the topic. - * @param msg_definition text obtained by either: - * - topic_tools::ShapeShifter::getMessageDefinition() - * - rosbag::MessageInstance::getMessageDefinition() - * - ros::message_traits::Definition< __your_type__ >::value() - * */ - Parser(const std::string& topic_name, const ROSType& msg_type, - const std::string& definition); - - enum MaxArrayPolicy : bool - { - DISCARD_LARGE_ARRAYS = true, - KEEP_LARGE_ARRAYS = false + std::vector> blob_storage; }; - /// Default values are DISCARD_LARGE_ARRAYS and 100. - /// The maximum permissible value of max_array_size is 10.000 (but don't) - void setMaxArrayPolicy(MaxArrayPolicy discard_entire_array, size_t max_array_size) + class Parser { - _discard_large_array = discard_entire_array; - _max_array_size = max_array_size; - if (_max_array_size > 10000) + public: + /** + * + * @param topic_name name of the topic to be used as node of the StringTree + * @param msg_type message type of the topic. + * @param msg_definition text obtained by either: + * - topic_tools::ShapeShifter::getMessageDefinition() + * - rosbag::MessageInstance::getMessageDefinition() + * - ros::message_traits::Definition< __your_type__ >::value() + * */ + Parser(const std::string& topic_name, const ROSType& msg_type, + const std::string& definition); + + enum MaxArrayPolicy : bool + { + DISCARD_LARGE_ARRAYS = true, + KEEP_LARGE_ARRAYS = false + }; + + /// Default values are DISCARD_LARGE_ARRAYS and 100. + /// The maximum permissible value of max_array_size is 10.000 (but don't) + void setMaxArrayPolicy(MaxArrayPolicy discard_entire_array, size_t max_array_size) { - throw std::runtime_error("max_array_size limited to 10000 at most"); + _discard_large_array = discard_entire_array; + _max_array_size = max_array_size; + if (_max_array_size > 10000) + { + throw std::runtime_error("max_array_size limited to 10000 at most"); + } } - } - MaxArrayPolicy maxArrayPolicy() const - { - return _discard_large_array; - } + MaxArrayPolicy maxArrayPolicy() const + { + return _discard_large_array; + } - size_t maxArraySize() const - { - return _max_array_size; - } + size_t maxArraySize() const + { + return _max_array_size; + } - enum BlobPolicy - { - STORE_BLOB_AS_COPY, - STORE_BLOB_AS_REFERENCE - }; + enum BlobPolicy + { + STORE_BLOB_AS_COPY, + STORE_BLOB_AS_REFERENCE + }; + + // If set to STORE_BLOB_AS_COPY, a copy of the original vector will be stored in the + // FlatMessage. This may have a large impact on performance. if STORE_BLOB_AS_REFERENCE + // is used instead, it is dramatically faster, but you must be careful with dangling + // pointers. + void setBlobPolicy(BlobPolicy policy) + { + _blob_policy = policy; + } - // If set to STORE_BLOB_AS_COPY, a copy of the original vector will be stored in the - // FlatMessage. This may have a large impact on performance. if STORE_BLOB_AS_REFERENCE - // is used instead, it is dramatically faster, but you must be careful with dangling - // pointers. - void setBlobPolicy(BlobPolicy policy) - { - _blob_policy = policy; - } + BlobPolicy blobPolicy() const + { + return _blob_policy; + } - BlobPolicy blobPolicy() const - { - return _blob_policy; - } - - /** - * @brief getSchema provides some metadata amout a registered ROSMessage. - */ - const std::shared_ptr& getSchema() const; - - ROSMessage::Ptr getMessageByType(const ROSType& type) const; - - /** - * @brief deserializeIntoFlatContainer takes a raw buffer of memory - * and extract information from it. - * This data is stored in two key/value vectors, FlatMessage::value and - * FlatMessage::name. It must be noted that the key type is FieldsVector. This type is - * not particularly user-friendly, but allows a much faster post-processing. - * - * IMPORTANT: this approach is not meant to be used with use arrays such as maps, - * point clouds and images.For this reason the argument max_array_size is used. - * - * This funtion is almost always followed by CreateRenamedValues, - * which provide a more human-readable key-value representation. - * - * @param buffer raw memory to be parsed. - * @param flat_output output to store the result. It is recommended to - * reuse the same object multiple times to - * avoid memory allocations and speed up the parsing. - * - * @return true if the entire message was parsed or false if parts of the message were - * skipped because an array has (size > max_array_size) - */ - bool deserialize(Span buffer, FlatMessage* flat_output, - Deserializer* deserializer) const; - - typedef std::function&)> VisitingCallback; - - /** - * @brief applyVisitorToBuffer is used to pass a callback that is invoked every time - * a chunk of memory storing an instance of ROSType = monitored_type - * is reached. - * Note that the VisitingCallback can modify the original message, but can NOT - * change its size. This means that strings and vectors can not be change their - * length. - * - * @param msg_identifier String ID to identify the registered message (use - * registerMessageDefinition first). - * @param monitored_type ROSType that triggers the invokation to the callback - * @param buffer Original buffer, passed as mutable since it might be - * modified. - * @param callback The callback. - */ - void applyVisitorToBuffer(const ROSType& msg_type, Span& buffer, - VisitingCallback callback) const; - - /// Change where the warning messages are displayed. - void setWarningsStream(std::ostream* output) - { - _global_warnings = output; - } + /** + * @brief getSchema provides some metadata amout a registered ROSMessage. + */ + const std::shared_ptr& getSchema() const; + + ROSMessage::Ptr getMessageByType(const ROSType& type) const; + + /** + * @brief deserializeIntoFlatContainer takes a raw buffer of memory + * and extract information from it. + * This data is stored in two key/value vectors, FlatMessage::value and + * FlatMessage::name. It must be noted that the key type is FieldsVector. This type is + * not particularly user-friendly, but allows a much faster post-processing. + * + * IMPORTANT: this approach is not meant to be used with use arrays such as maps, + * point clouds and images.For this reason the argument max_array_size is used. + * + * This funtion is almost always followed by CreateRenamedValues, + * which provide a more human-readable key-value representation. + * + * @param buffer raw memory to be parsed. + * @param flat_output output to store the result. It is recommended to + * reuse the same object multiple times to + * avoid memory allocations and speed up the parsing. + * + * @return true if the entire message was parsed or false if parts of the message were + * skipped because an array has (size > max_array_size) + */ + bool deserialize(Span buffer, FlatMessage* flat_output, + Deserializer* deserializer) const; + + bool deserializeIntoJson(Span buffer, + std::string *json_txt, + Deserializer *deserializer, + bool ignore_constants = true) const; + + bool serializeFromJson(std::vector &bufferOut, + std::string *json_txt) const; + + typedef std::function&)> VisitingCallback; + + /** + * @brief applyVisitorToBuffer is used to pass a callback that is invoked every time + * a chunk of memory storing an instance of ROSType = monitored_type + * is reached. + * Note that the VisitingCallback can modify the original message, but can NOT + * change its size. This means that strings and vectors can not be change their + * length. + * + * @param msg_identifier String ID to identify the registered message (use + * registerMessageDefinition first). + * @param monitored_type ROSType that triggers the invokation to the callback + * @param buffer Original buffer, passed as mutable since it might be + * modified. + * @param callback The callback. + */ + void applyVisitorToBuffer(const ROSType& msg_type, Span& buffer, + VisitingCallback callback) const; + + /// Change where the warning messages are displayed. + void setWarningsStream(std::ostream* output) + { + _global_warnings = output; + } -private: - std::shared_ptr _schema; + private: + std::shared_ptr _schema; - std::ostream* _global_warnings; + std::ostream* _global_warnings; - std::string _topic_name; + std::string _topic_name; - std::vector _alias_array_pos; - std::vector _formatted_string; - std::vector _substituted; - MaxArrayPolicy _discard_large_array; - size_t _max_array_size; - BlobPolicy _blob_policy; - std::shared_ptr _dummy_root_field; + std::vector _alias_array_pos; + std::vector _formatted_string; + std::vector _substituted; + MaxArrayPolicy _discard_large_array; + size_t _max_array_size; + BlobPolicy _blob_policy; + std::shared_ptr _dummy_root_field; - std::unique_ptr _deserializer; -}; + std::unique_ptr _deserializer; + }; -typedef std::vector> RenamedValues; + typedef std::vector> RenamedValues; -template -class ParsersCollection -{ -public: - ParsersCollection() + template + class ParsersCollection { - _deserializer = std::make_unique(); - } + public: + ParsersCollection() + { + _deserializer = std::make_unique(); + } - void registerParser(const std::string& topic_name, const ROSType& msg_type, - const std::string& definition) - { - if (_pack.count(topic_name) == 0) + void registerParser(const std::string& topic_name, const ROSType& msg_type, + const std::string& definition) { - Parser parser(topic_name, msg_type, definition); - CachedPack pack = { std::move(parser), {} }; - _pack.insert({ topic_name, std::move(pack) }); + if (_pack.count(topic_name) == 0) + { + Parser parser(topic_name, msg_type, definition); + CachedPack pack = { std::move(parser), {} }; + _pack.insert({ topic_name, std::move(pack) }); + + // TODO: I think better to be a ptr to parser, to not replicate the same object + Parser parserJ(topic_name, msg_type, definition); + CachedPackJSON packJ = {std::move(parserJ), "",{}}; + _packJSON.insert({topic_name, std::move(packJ)}); + } } - } - const Parser* getParser(const std::string& topic_name) const - { - auto it = _pack.find(topic_name); - if (it != _pack.end()) + const Parser* getParser(const std::string& topic_name) const { - return &it->second.parser; + auto it = _pack.find(topic_name); + if (it != _pack.end()) + { + return &it->second.parser; + } + return nullptr; } - return nullptr; - } - const FlatMessage* deserialize(const std::string& topic_name, - Span buffer) - { - auto it = _pack.find(topic_name); - if (it != _pack.end()) + const FlatMessage* deserialize(const std::string& topic_name, + Span buffer) + { + auto it = _pack.find(topic_name); + if (it != _pack.end()) + { + CachedPack& pack = it->second; + Parser& parser = pack.parser; + + parser.deserialize(buffer, &pack.msg, _deserializer.get()); + return &pack.msg; + } + return nullptr; + } + + std::string *deserializeIntoJson(const std::string &topic_name, + Span buffer, bool ignore_constants) { - CachedPack& pack = it->second; - Parser& parser = pack.parser; + auto it = _packJSON.find(topic_name); + if (it != _packJSON.end()) + { + + CachedPackJSON &pack = it->second; + Parser &parser = pack.parser; - parser.deserialize(buffer, &pack.msg, _deserializer.get()); - return &pack.msg; + parser.deserializeIntoJson(buffer, &pack.msg, _deserializer.get(), ignore_constants); + return &pack.msg; + } + return nullptr; } - return nullptr; - } -private: - struct CachedPack - { - Parser parser; - FlatMessage msg; + const std::vector serializeFromJson(const std::string &topic_name, + std::string *json_txt ) + { + auto it = _packJSON.find(topic_name); + if (it != _packJSON.end()) + { + + CachedPackJSON &pack = it->second; + Parser &parser = pack.parser; + + parser.serializeFromJson(pack.buffer,json_txt ); + return pack.buffer; + } + return {}; + } + + private: + struct CachedPack + { + Parser parser; + FlatMessage msg; + }; + + struct CachedPackJSON + { + Parser parser; + std::string msg; + std::vector buffer; + }; + std::unordered_map _pack; + std::unordered_map _packJSON; + std::vector _buffer; + std::unique_ptr _deserializer; }; - std::unordered_map _pack; - std::vector _buffer; - std::unique_ptr _deserializer; -}; } // namespace RosMsgParser diff --git a/include/rosx_introspection/ros_utils/ros2_helpers.hpp b/include/rosx_introspection/ros_utils/ros2_helpers.hpp index 1f3d023..539c349 100644 --- a/include/rosx_introspection/ros_utils/ros2_helpers.hpp +++ b/include/rosx_introspection/ros_utils/ros2_helpers.hpp @@ -1,5 +1,5 @@ -#ifndef ROS1_HELPERS_HPP -#define ROS1_HELPERS_HPP +#ifndef ROS2_HELPERS_HPP +#define ROS2_HELPERS_HPP #include "rosx_introspection/ros_field.hpp" @@ -85,4 +85,4 @@ std::vector BuildMessageBuffer(const T& msg, const std::string& topic_t } -#endif // ROS1_HELPERS_HPP +#endif // ROS2_HELPERS_HPP diff --git a/src/ros_parser.cpp b/src/ros_parser.cpp index c25291e..6031c3e 100644 --- a/src/ros_parser.cpp +++ b/src/ros_parser.cpp @@ -28,220 +28,759 @@ namespace RosMsgParser { -inline bool operator==(const std::string& a, const std::string_view& b) -{ - return (a.size() == b.size() && std::strncmp(a.data(), b.data(), a.size()) == 0); -} - -Parser::Parser(const std::string& topic_name, const ROSType& msg_type, - const std::string& definition) - : _global_warnings(&std::cerr) - , _topic_name(topic_name) - , _discard_large_array(DISCARD_LARGE_ARRAYS) - , _max_array_size(100) - , _blob_policy(STORE_BLOB_AS_COPY) - , _dummy_root_field(new ROSField(msg_type, topic_name)) -{ - auto parsed_msgs = ParseMessageDefinitions(definition, msg_type); - _schema = BuildMessageSchema(topic_name, parsed_msgs); -} + inline bool operator==(const std::string& a, const std::string_view& b) + { + return (a.size() == b.size() && std::strncmp(a.data(), b.data(), a.size()) == 0); + } -const std::shared_ptr& Parser::getSchema() const -{ - return _schema; -} + Parser::Parser(const std::string& topic_name, const ROSType& msg_type, + const std::string& definition) + : _global_warnings(&std::cerr) + , _topic_name(topic_name) + , _discard_large_array(DISCARD_LARGE_ARRAYS) + , _max_array_size(100) + , _blob_policy(STORE_BLOB_AS_COPY) + , _dummy_root_field(new ROSField(msg_type, topic_name)) + { + auto parsed_msgs = ParseMessageDefinitions(definition, msg_type); + _schema = BuildMessageSchema(topic_name, parsed_msgs); + } -ROSMessage::Ptr Parser::getMessageByType(const ROSType& type) const -{ - for (const auto& [msg_type, msg] : _schema->msg_library) // find in the list + const std::shared_ptr& Parser::getSchema() const { - if (msg_type == type) + return _schema; + } + + ROSMessage::Ptr Parser::getMessageByType(const ROSType& type) const + { + for (const auto& [msg_type, msg] : _schema->msg_library) // find in the list { - return msg; + if (msg_type == type) + { + return msg; + } } + return {}; } - return {}; -} -template -inline void ExpandVectorIfNecessary(Container& container, size_t new_size) -{ - if (container.size() <= new_size) + template + inline void ExpandVectorIfNecessary(Container& container, size_t new_size) { - const size_t increased_size = std::max(size_t(32), container.size() * 2); - container.resize(increased_size); + if (container.size() <= new_size) + { + const size_t increased_size = std::max(size_t(32), container.size() * 2); + container.resize(increased_size); + } } -} -bool Parser::deserialize(Span buffer, FlatMessage* flat_container, - Deserializer* deserializer) const -{ - deserializer->init(buffer); - - bool entire_message_parse = true; + bool Parser::deserializeIntoJson(Span buffer, std::string *json_txt, Deserializer *deserializer, bool ignore_constants) const + { + deserializer->init(buffer); - size_t value_index = 0; - size_t name_index = 0; - size_t blob_index = 0; - size_t blob_storage_index = 0; + rapidjson::Document json_document; + rapidjson::Document::AllocatorType &alloc = json_document.GetAllocator(); - std::function deserializeImpl; + size_t buffer_offset = 0; - deserializeImpl = [&](const ROSMessage* msg, FieldLeaf tree_leaf, bool store) { - size_t index_s = 0; - size_t index_m = 0; + std::function deserializeImpl; - for (const ROSField& field : msg->fields()) + deserializeImpl = [&](const ROSMessage *msg_node, rapidjson::Value &json_value) { - bool DO_STORE = store; - if (field.isConstant()) - { - continue; - } + // const ROSMessage *msg_definition = msg_node->value(); + size_t index_s = 0; + size_t index_m = 0; - const ROSType& field_type = field.type(); + for (const ROSField &field : msg_node->fields()) + { + if (field.isConstant() && ignore_constants) + continue; - auto new_tree_leaf = tree_leaf; - new_tree_leaf.node = tree_leaf.node->child(index_s); + const ROSType &field_type = field.type(); + auto field_name = rapidjson::StringRef(field.name().data(), field.name().size()); - int32_t array_size = field.arraySize(); - if (array_size == -1) - { - array_size = deserializer->deserializeUInt32(); - } - if (field.isArray()) - { - new_tree_leaf.index_array.push_back(0); - } + int32_t array_size = field.arraySize(); + if (array_size == -1) + { + array_size = deserializer->deserializeUInt32(); - bool IS_BLOB = false; + // ReadFromBuffer(buffer, buffer_offset, array_size); + } - // Stop storing it if is NOT a blob and a very large array. - if (array_size > static_cast(_max_array_size) && - field_type.typeID() == BuiltinType::OTHER) - { - if (builtinSize(field_type.typeID()) == 1) + // Stop storing if it is a blob. + if (array_size > static_cast(_max_array_size)) { - IS_BLOB = true; + if (buffer_offset + array_size > static_cast(buffer.size())) + { + throw std::runtime_error("Buffer overrun in blob"); + } + buffer_offset += array_size; } - else + else // NOT a BLOB { - if (_discard_large_array) + rapidjson::Value array_value(rapidjson::kArrayType); + + for (int i = 0; i < array_size; i++) { - DO_STORE = false; + rapidjson::Value new_value; + new_value.SetObject(); + + switch (field_type.typeID()) + { + case BOOL: + // new_value.SetBool(ReadFromBuffer(buffer, buffer_offset)); + new_value.SetBool(deserializer->deserialize(field_type.typeID()).convert()); + break; + case CHAR: + { + char c = deserializer->deserialize(field_type.typeID()).convert(); + new_value.SetString(&c, 1, alloc); + } + break; + case BYTE: + case UINT8: + new_value.SetUint((deserializer->deserialize(field_type.typeID()).convert())); + break; + case UINT16: + new_value.SetUint((deserializer->deserialize(field_type.typeID()).convert())); + break; + case UINT32: + new_value.SetUint((deserializer->deserialize(field_type.typeID()).convert())); + break; + case UINT64: + new_value.SetUint64((deserializer->deserialize(field_type.typeID()).convert())); + break; + case INT8: + new_value.SetInt((deserializer->deserialize(field_type.typeID()).convert())); + break; + case INT16: + new_value.SetInt((deserializer->deserialize(field_type.typeID()).convert())); + break; + case INT32: + new_value.SetInt((deserializer->deserialize(field_type.typeID()).convert())); + break; + case INT64: + new_value.SetInt64((deserializer->deserialize(field_type.typeID()).convert())); + break; + case FLOAT32: + new_value.SetFloat((deserializer->deserialize(field_type.typeID()).convert())); + break; + case FLOAT64: + new_value.SetDouble((deserializer->deserialize(field_type.typeID()).convert())); + break; + case TIME: + { + int sec = deserializer->deserialize(INT32).convert(); + int nsec = deserializer->deserialize(INT32).convert(); + + // ReadFromBuffer(buffer, buffer_offset, tmp.sec); + // ReadFromBuffer(buffer, buffer_offset, tmp.nsec); + + rapidjson::Value sec_Value; + sec_Value.SetObject(); + sec_Value.SetInt(sec); + new_value.AddMember("secs", sec_Value, alloc); + + rapidjson::Value nsec_value; + nsec_value.SetObject(); + nsec_value.SetInt(nsec); + new_value.AddMember("nsecs", nsec_value, alloc); + } + break; + case DURATION: + { + int sec = deserializer->deserialize(INT32).convert(); + int nsec = deserializer->deserialize(INT32).convert(); + + // ReadFromBuffer(buffer, buffer_offset, tmp.sec); + // ReadFromBuffer(buffer, buffer_offset, tmp.nsec); + + rapidjson::Value sec_Value; + sec_Value.SetObject(); + sec_Value.SetInt(sec); + new_value.AddMember("secs", sec_Value, alloc); + + rapidjson::Value nsec_value; + nsec_value.SetObject(); + nsec_value.SetInt(nsec); + new_value.AddMember("nsecs", nsec_value, alloc); + } + break; + + case STRING: + { + std::string s; + deserializer->deserializeString(s); + + // uint32_t string_size = 0; + // ReadFromBuffer(buffer, buffer_offset, string_size); + // if (buffer_offset + string_size > static_cast(buffer.size())) + // { + // throw std::runtime_error("Buffer overrun"); + // } + new_value.SetString(s.c_str(), s.length(), alloc); + // buffer_offset += string_size; + } + break; + case OTHER: + { + + auto msg_node_child = field.getMessagePtr(_schema->msg_library); + // deserializeImpl(msg_node.get(), new_tree_leaf, DO_STORE_ARRAY); + + deserializeImpl(msg_node_child.get(), new_value); + } + break; + } // end switch + + if (field.isArray()) + { + array_value.PushBack(new_value, alloc); + } + else + { + json_value.AddMember(field_name, new_value, alloc); + } + } // end for array + + if (field.isArray()) + { + json_value.AddMember(field_name, array_value, alloc); } - entire_message_parse = false; + } // end for array_size + + if (field_type.typeID() == OTHER) + { + index_m++; } - } + index_s++; + } // end for fields + }; // end of lambda - if (IS_BLOB) // special case. This is a "blob", typically an image, a map, - // pointcloud, etc. + // pass the shared_ptr + + FieldLeaf rootnode; + rootnode.node = _schema->field_tree.croot(); + auto root_msg = + _schema->field_tree.croot()->value()->getMessagePtr(_schema->msg_library); + + json_document.SetObject(); + rapidjson::Value json_node; + json_node.SetObject(); + + deserializeImpl(root_msg.get(), json_node); + + auto topic_name = rapidjson::StringRef(_topic_name.data(), _topic_name.size()); + json_document.AddMember("topic", topic_name, alloc); + json_document.AddMember("msg", json_node, alloc); + + rapidjson::StringBuffer json_buffer; + json_buffer.Reserve(2048); + + rapidjson::Writer, + rapidjson::UTF8<>, + rapidjson::CrtAllocator, + rapidjson::kWriteDefaultFlags | + rapidjson::kWriteNanAndInfFlag> + json_writer(json_buffer); + json_document.Accept(json_writer); + + *json_txt = json_buffer.GetString(); + + return true; + } + + bool Parser::serializeFromJson(std::vector &bufferOut, std::string *json_txt) const + { + rapidjson::Document json_document; + json_document.Parse(json_txt->c_str()); + rapidjson::Document::AllocatorType &alloc = json_document.GetAllocator(); + + uint8_t *buffer_data; + + size_t buffer_offset = 0; + + std::function deserializeImpl; + // rapidjson::Value new_value; + + deserializeImpl = [&](const ROSMessage *msg_node, rapidjson::Value &new_value) + { + // const ROSMessage *msg_definition = msg_node->value(); + size_t index_s = 0; + size_t index_m = 0; + + for (const ROSField &field : msg_node->fields()) { - ExpandVectorIfNecessary(flat_container->blob, blob_index); - if (array_size > deserializer->bytesLeft()) + // if (field.isConstant()) + // continue; + + const ROSType &field_type = field.type(); + auto field_name = rapidjson::StringRef(field.name().data(), field.name().size()); + + uint32_t array_size = field.arraySize(); + + if (array_size == -1) { - throw std::runtime_error("Buffer overrun in deserializeIntoFlatContainer " - "(blob)"); + if (!new_value.HasMember(field_name.s)) + { + + throw std::runtime_error("looks like it is a blob that wasn't serialized"); + } + + array_size = new_value[field_name.s].GetArray().Size(); + buffer_data = reinterpret_cast(&array_size); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(uint32_t)); } - if (DO_STORE) + + for (int i = 0; i < array_size; i++) { - flat_container->blob[blob_index].first = FieldsVector(new_tree_leaf); - auto& blob = flat_container->blob[blob_index].second; - blob_index++; - if (_blob_policy == STORE_BLOB_AS_COPY) + switch (field_type.typeID()) + { + case BOOL: + { + bool val; + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetBool(); + } + else + { + val = new_value[field_name.s].GetBool(); + } + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(bool)); + } + break; + case CHAR: { - ExpandVectorIfNecessary(flat_container->blob_storage, blob_storage_index); - auto& storage = flat_container->blob_storage[blob_storage_index]; - storage.resize(array_size); - std::memcpy(storage.data(), deserializer->getCurrentPtr(), array_size); - blob_storage_index++; + char val; + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetString()[0]; + } + else + { + val = new_value[field_name.s].GetString()[0]; + } + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(char)); + } + break; + case BYTE: + case UINT8: + { - blob = Span(storage.data(), storage.size()); + uint8_t val; + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetUint(); + } + else + { + val = new_value[field_name.s].GetUint(); + } + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(uint8_t)); } - else + break; + case UINT16: { - blob = Span(deserializer->getCurrentPtr(), array_size); + uint16_t val; + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetUint(); + } + else + { + val = new_value[field_name.s].GetUint(); + } + + buffer_data = reinterpret_cast(&val); + + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(uint16_t)); } - } - deserializer->jump(array_size); - } - else // NOT a BLOB - { - bool DO_STORE_ARRAY = DO_STORE; - for (int i = 0; i < array_size; i++) - { - if (DO_STORE_ARRAY && i >= static_cast(_max_array_size)) + break; + case UINT32: { - DO_STORE_ARRAY = false; + + uint32_t val; + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetUint(); + } + else + { + val = new_value[field_name.s].GetUint(); + } + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(uint32_t)); } + break; + case UINT64: + { - if (field.isArray() && DO_STORE_ARRAY) + uint64_t val; + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetUint64(); + } + else + { + val = new_value[field_name.s].GetUint64(); + } + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(uint64_t)); + } + break; + case INT8: + { + int8_t val; + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetInt(); + } + else + { + val = new_value[field_name.s].GetInt(); + } + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(int8_t)); + } + break; + case INT16: + { + int16_t val; + + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetInt(); + } + else + { + val = new_value[field_name.s].GetInt(); + } + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(int16_t)); + } + break; + case INT32: + { + int32_t val; + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetInt(); + } + else + { + val = new_value[field_name.s].GetInt(); + } + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(int32_t)); + } + break; + case INT64: + { + int64_t val; + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetInt64(); + } + else + { + val = new_value[field_name.s].GetInt64(); + } + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(int64_t)); + } + break; + case FLOAT32: + { + float val; + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetFloat(); + } + else + { + val = new_value[field_name.s].GetFloat(); + } + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(float)); + } + break; + case FLOAT64: + { + double val; + if (new_value[field_name.s].IsArray()) + { + val = new_value[field_name.s].GetArray()[i].GetDouble(); + } + else + { + val = new_value[field_name.s].GetDouble(); + } + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(double)); + } + break; + case TIME: + { + + int val = new_value[field_name.s].GetObject()["secs"].GetInt(); + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(uint32_t)); + + val = new_value[field_name.s].GetObject()["nsecs"].GetInt(); + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(uint32_t)); + } + break; + case DURATION: { - new_tree_leaf.index_array.back() = i; + + int val = new_value[field_name.s].GetObject()["secs"].GetInt(); + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(uint32_t)); + + val = new_value[field_name.s].GetObject()["nsecs"].GetInt(); + buffer_data = reinterpret_cast(&val); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(uint32_t)); } + break; - if (field_type.typeID() == STRING) + case STRING: { - ExpandVectorIfNecessary(flat_container->name, name_index); - std::string str; - deserializer->deserializeString(str); + char *val; + uint32_t len; - if (DO_STORE_ARRAY) + if (new_value[field_name.s].IsArray()) { - flat_container->name[name_index].first = FieldsVector(new_tree_leaf); - flat_container->name[name_index].second = str; - name_index++; + val = (char *)new_value[field_name.s].GetArray()[i].GetString(); + len = new_value[field_name.s].GetArray()[i].GetStringLength(); } + else + { + val = (char *)new_value[field_name.s].GetString(); + len = new_value[field_name.s].GetStringLength(); + } + + buffer_data = reinterpret_cast(&len); + bufferOut.insert(bufferOut.end(), buffer_data, buffer_data + sizeof(uint32_t)); + + bufferOut.insert(bufferOut.end(), val, val + len); } - else if (field_type.isBuiltin()) + break; + case OTHER: { - ExpandVectorIfNecessary(flat_container->value, value_index); + rapidjson::Value::Object new_value2 = new_value[field_name.s].GetObject(); + + auto msg_node_child = field.getMessagePtr(_schema->msg_library); + + deserializeImpl(msg_node_child.get(), new_value2); + // deserializeImpl(msg_node->child(index_m), new_value2); + } + break; + } // end switch + + } // end for array + + if (field_type.typeID() == OTHER) + { + index_m++; + } + index_s++; + } // end for fields + }; // end of lambda + + // pass the shared_ptr + + rapidjson::Value json_node; + json_node = json_document.GetObject(); + + FieldLeaf rootnode; + rootnode.node = _schema->field_tree.croot(); + auto root_msg = + _schema->field_tree.croot()->value()->getMessagePtr(_schema->msg_library); + + deserializeImpl(root_msg.get(), json_node); + // deserializeImpl(_message_info->message_tree.croot(), json_node); + + return true; + } + + bool Parser::deserialize(Span buffer, FlatMessage* flat_container, + Deserializer* deserializer) const + { + deserializer->init(buffer); + + bool entire_message_parse = true; + + size_t value_index = 0; + size_t name_index = 0; + size_t blob_index = 0; + size_t blob_storage_index = 0; + + std::function deserializeImpl; + + deserializeImpl = [&](const ROSMessage* msg, FieldLeaf tree_leaf, bool store) { + size_t index_s = 0; + size_t index_m = 0; + + for (const ROSField& field : msg->fields()) + { + bool DO_STORE = store; + if (field.isConstant()) + { + continue; + } + + const ROSType& field_type = field.type(); - Variant var = deserializer->deserialize(field_type.typeID()); - if (DO_STORE_ARRAY) + auto new_tree_leaf = tree_leaf; + new_tree_leaf.node = tree_leaf.node->child(index_s); + + int32_t array_size = field.arraySize(); + if (array_size == -1) + { + array_size = deserializer->deserializeUInt32(); + } + if (field.isArray()) + { + new_tree_leaf.index_array.push_back(0); + } + + bool IS_BLOB = false; + + // Stop storing it if is NOT a blob and a very large array. + if (array_size > static_cast(_max_array_size) && + field_type.typeID() == BuiltinType::OTHER) + { + if (builtinSize(field_type.typeID()) == 1) + { + IS_BLOB = true; + } + else + { + if (_discard_large_array) { - flat_container->value[value_index] = - std::make_pair(new_tree_leaf, std::move(var)); - value_index++; + DO_STORE = false; } + entire_message_parse = false; } - else - { // field_type.typeID() == OTHER - auto msg_node = field.getMessagePtr(_schema->msg_library); - deserializeImpl(msg_node.get(), new_tree_leaf, DO_STORE_ARRAY); + } + + if (IS_BLOB) // special case. This is a "blob", typically an image, a map, + // pointcloud, etc. + { + ExpandVectorIfNecessary(flat_container->blob, blob_index); + + if (array_size > deserializer->bytesLeft()) + { + throw std::runtime_error("Buffer overrun in deserializeIntoFlatContainer " + "(blob)"); } - } // end for array_size - } + if (DO_STORE) + { + flat_container->blob[blob_index].first = FieldsVector(new_tree_leaf); + auto& blob = flat_container->blob[blob_index].second; + blob_index++; - if (field_type.typeID() == OTHER) - { - index_m++; - } - index_s++; - } // end for fields - }; // end of lambda + if (_blob_policy == STORE_BLOB_AS_COPY) + { + ExpandVectorIfNecessary(flat_container->blob_storage, blob_storage_index); - // pass the shared_ptr - flat_container->schema = _schema; + auto& storage = flat_container->blob_storage[blob_storage_index]; + storage.resize(array_size); + std::memcpy(storage.data(), deserializer->getCurrentPtr(), array_size); + blob_storage_index++; - FieldLeaf rootnode; - rootnode.node = _schema->field_tree.croot(); - auto root_msg = - _schema->field_tree.croot()->value()->getMessagePtr(_schema->msg_library); + blob = Span(storage.data(), storage.size()); + } + else + { + blob = Span(deserializer->getCurrentPtr(), array_size); + } + } + deserializer->jump(array_size); + } + else // NOT a BLOB + { + bool DO_STORE_ARRAY = DO_STORE; + for (int i = 0; i < array_size; i++) + { + if (DO_STORE_ARRAY && i >= static_cast(_max_array_size)) + { + DO_STORE_ARRAY = false; + } - deserializeImpl(root_msg.get(), rootnode, true); + if (field.isArray() && DO_STORE_ARRAY) + { + new_tree_leaf.index_array.back() = i; + } - flat_container->name.resize(name_index); - flat_container->value.resize(value_index); - flat_container->blob.resize(blob_index); - flat_container->blob_storage.resize(blob_storage_index); + if (field_type.typeID() == STRING) + { + ExpandVectorIfNecessary(flat_container->name, name_index); - return entire_message_parse; -} + std::string str; + deserializer->deserializeString(str); + + if (DO_STORE_ARRAY) + { + flat_container->name[name_index].first = FieldsVector(new_tree_leaf); + flat_container->name[name_index].second = str; + name_index++; + } + } + else if (field_type.isBuiltin()) + { + ExpandVectorIfNecessary(flat_container->value, value_index); + + Variant var = deserializer->deserialize(field_type.typeID()); + if (DO_STORE_ARRAY) + { + flat_container->value[value_index] = + std::make_pair(new_tree_leaf, std::move(var)); + value_index++; + } + } + else + { // field_type.typeID() == OTHER + auto msg_node = field.getMessagePtr(_schema->msg_library); + deserializeImpl(msg_node.get(), new_tree_leaf, DO_STORE_ARRAY); + } + } // end for array_size + } + + if (field_type.typeID() == OTHER) + { + index_m++; + } + index_s++; + } // end for fields + }; // end of lambda + + // pass the shared_ptr + flat_container->schema = _schema; + + FieldLeaf rootnode; + rootnode.node = _schema->field_tree.croot(); + auto root_msg = + _schema->field_tree.croot()->value()->getMessagePtr(_schema->msg_library); + + deserializeImpl(root_msg.get(), rootnode, true); + + flat_container->name.resize(name_index); + flat_container->value.resize(value_index); + flat_container->blob.resize(blob_index); + flat_container->blob_storage.resize(blob_storage_index); + + return entire_message_parse; + } } // namespace RosMsgParser diff --git a/test/test_ros1.cpp b/test/test_ros1.cpp index 251661b..e0b62a8 100644 --- a/test/test_ros1.cpp +++ b/test/test_ros1.cpp @@ -84,6 +84,116 @@ TEST_CASE("Parse ROS1 [JointState]") CHECK( flat_container->value[10].second.convert() == 52 ); + CHECK( flat_container->name[0].first.toStdString() == ("joint_state/header/frame_id")); + CHECK( flat_container->name[0].second == ("pippo") ); + + CHECK( flat_container->name[1].first.toStdString() == ("joint_state/name[0]")); + CHECK( flat_container->name[1].second == ("hola") ); + + CHECK( flat_container->name[2].first.toStdString() == ("joint_state/name[1]")); + CHECK( flat_container->name[2].second == ("ciao") ); + + CHECK( flat_container->name[3].first.toStdString() == ("joint_state/name[2]")); + CHECK( flat_container->name[3].second == ("bye") ); +} + +TEST_CASE("Parse ROS1 JSON [JointState]") +{ + ParsersCollection parser; + parser.registerParser("joint_state", + GetMessageType(), + GetMessageDefinition()); + sensor_msgs::JointState joint_state; + + joint_state.header.seq = 2016; + joint_state.header.stamp.sec = 1234; + joint_state.header.stamp.nsec = 567 * 1000 * 1000; + joint_state.header.frame_id = "pippo"; + + joint_state.name.resize(3); + joint_state.position.resize(3); + joint_state.velocity.resize(3); + joint_state.effort.resize(3); + + std::string names[3]; + names[0] = ("hola"); + names[1] = ("ciao"); + names[2] = ("bye"); + + for (int i = 0; i < 3; i++) + { + joint_state.name[i] = names[i]; + joint_state.position[i] = 10 + i; + joint_state.velocity[i] = 30 + i; + joint_state.effort[i] = 50 + i; + } + + std::vector buffer = BuildMessageBuffer(joint_state); + + std::string* json_txt = parser.deserializeIntoJson("joint_state", Span(buffer), false); + // parse the string from format {topic:{t} , msg: {m}} into {m} + size_t startIdx = json_txt->find("msg"); + if (startIdx != std::string::npos) + { + startIdx += 6; // Move past "(msg:)" + + // Find the ending index before the last "}" + size_t endIdx = json_txt->rfind("}"); + if (endIdx != std::string::npos) + { + // Extract the substring + *json_txt = "{" + json_txt->substr(startIdx, endIdx - startIdx); + } + else + { + std::cerr << "Error: Unable to find the last '}'." << std::endl; + } + } + else + { + std::cerr << "Error: Unable to find '(msg:)'" << std::endl; + } + + std::vector bufferOut= parser.serializeFromJson("joint_state", json_txt); + + ros::serialization::IStream sstream(&bufferOut[0], bufferOut.size()); + sensor_msgs::JointState joint_state_from_json; + ros::serialization::deserialize(sstream, joint_state_from_json); + + + auto flat_container = parser.deserialize("joint_state", Span(bufferOut) ); + + CHECK( flat_container->value[0].first.toStdString() == ("joint_state/header/seq")); + CHECK( flat_container->value[0].second.convert() == 2016 ); + + CHECK( flat_container->value[1].first.toStdString() == ("joint_state/header/stamp")); + CHECK( flat_container->value[1].second.convert() - 1234.567<=0.01 ); + auto time = flat_container->value[1].second.convert