ros_parser.hpp

Go to the documentation of this file.

/***** MIT License ****
 *
 *   Copyright (c) 2016-2022 Davide Faconti
 *
 *   Permission is hereby granted, free of charge, to any person obtaining a copy
 *   of this software and associated documentation files (the "Software"), to deal
 *   in the Software without restriction, including without limitation the rights
 *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *   copies of the Software, and to permit persons to whom the Software is
 *   furnished to do so, subject to the following conditions:
 *
 *   The above copyright notice and this permission notice shall be included in all
 *   copies or substantial portions of the Software.
 *
 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 *   SOFTWARE.
 */
#pragma once
 
#include <unordered_set>
 
#include "rosx_introspection/stringtree_leaf.hpp"
#include "rosx_introspection/deserializer.hpp"
 
namespace RosMsgParser
{
 
struct FlatMessage
{
  std::shared_ptr<MessageSchema> schema;
 
  std::vector<std::pair<FieldsVector, Variant>> value;
 
  std::vector<std::pair<FieldsVector, std::string>> name;
 
  std::vector<std::pair<FieldsVector, Span<const uint8_t>>> blob;
 
  std::vector<std::vector<uint8_t>> blob_storage;
};
 
class Parser
{
public:
  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
  };
 
  void setMaxArrayPolicy(MaxArrayPolicy discard_entire_array, size_t max_array_size)
  {
    _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;
  }
 
  size_t maxArraySize() const
  {
    return _max_array_size;
  }
 
  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;
  }
 
  BlobPolicy blobPolicy() const
  {
    return _blob_policy;
  }
 
  const std::shared_ptr<MessageSchema>& getSchema() const;
 
  ROSMessage::Ptr getMessageByType(const ROSType& type) const;
 
  bool deserialize(Span<const uint8_t> buffer, FlatMessage* flat_output,
                   Deserializer* deserializer) const;
 
  typedef std::function<void(const ROSType&, Span<uint8_t>&)> VisitingCallback;
 
  void applyVisitorToBuffer(const ROSType& msg_type, Span<uint8_t>& buffer,
                            VisitingCallback callback) const;
 
  void setWarningsStream(std::ostream* output)
  {
    _global_warnings = output;
  }
 
private:
  std::shared_ptr<MessageSchema> _schema;
 
  std::ostream* _global_warnings;
 
  std::string _topic_name;
 
  std::vector<int> _alias_array_pos;
  std::vector<std::string> _formatted_string;
  std::vector<int8_t> _substituted;
  MaxArrayPolicy _discard_large_array;
  size_t _max_array_size;
  BlobPolicy _blob_policy;
  std::shared_ptr<ROSField> _dummy_root_field;
 
  std::unique_ptr<Deserializer> _deserializer;
};
 
typedef std::vector<std::pair<std::string, double>> RenamedValues;
 
template <class DeserializerT>
class ParsersCollection
{
public:
  ParsersCollection()
  {
    _deserializer = std::make_unique<DeserializerT>();
  }
 
  void registerParser(const std::string& topic_name, const ROSType& msg_type,
                      const std::string& definition)
  {
    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) });
    }
  }
 
  const Parser* getParser(const std::string& topic_name) const
  {
    auto it = _pack.find(topic_name);
    if (it != _pack.end())
    {
      return &it->second.parser;
    }
    return nullptr;
  }
 
  const FlatMessage* deserialize(const std::string& topic_name,
                                 Span<const uint8_t> 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;
  }
 
private:
  struct CachedPack
  {
    Parser parser;
    FlatMessage msg;
  };
  std::unordered_map<std::string, CachedPack> _pack;
  std::vector<uint8_t> _buffer;
  std::unique_ptr<Deserializer> _deserializer;
};
 
}  // namespace RosMsgParser