ros_introspection.hpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright 2016-2017 Davide Faconti
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
* *******************************************************************/

#ifndef ROS_INTROSPECTION_HPP
#define ROS_INTROSPECTION_HPP

#include <unordered_set>
#include <ros_type_introspection/stringtree_leaf.hpp>
#include <ros_type_introspection/substitution_rule.hpp>
#include <ros_type_introspection/helper_functions.hpp>

namespace RosIntrospection{

struct FlatMessage {

  const StringTree* tree;

  std::vector< std::pair<StringTreeLeaf, Variant> > value;

  std::vector< std::pair<StringTreeLeaf, std::string> > name;

  std::vector< std::pair<StringTreeLeaf, Span<uint8_t>>> blob;

  std::vector<std::vector<uint8_t>> blob_storage;
};

typedef std::vector< std::pair<std::string, Variant> > RenamedValues;

class Parser{

public:
  Parser(): _rule_cache_dirty(true),
            _global_warnings(&std::cerr),
            _discard_large_array(DISCARD_LARGE_ARRAYS),
            _blob_policy(STORE_BLOB_AS_COPY)
 {}

  enum MaxArrayPolicy: bool {
    DISCARD_LARGE_ARRAYS = true,
    KEEP_LARGE_ARRAYS = false
  };

  void setMaxArrayPolicy( MaxArrayPolicy discard_entire_array )
  {
      _discard_large_array = discard_entire_array;
  }

  void setMaxArrayPolicy( bool discard_entire_array )
  {
    _discard_large_array = static_cast<MaxArrayPolicy>(discard_entire_array);
  }

  MaxArrayPolicy maxArrayPolicy() const
  {
    return _discard_large_array;
  }

  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 STOR_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;
  }

  void registerMessageDefinition(const std::string& message_identifier,
                                 const ROSType &main_type,
                                 const std::string& definition);

  void registerRenamingRules(const ROSType& type,
                             const std::vector<SubstitutionRule> &rules );

  const ROSMessageInfo* getMessageInfo(const std::string& msg_identifier) const;

  const ROSMessage *getMessageByType(const ROSType& type, const ROSMessageInfo &info) const;

  bool deserializeIntoFlatContainer(const std::string& msg_identifier,
                                    Span<uint8_t> buffer,
                                    FlatMessage* flat_container_output,
                                    const uint32_t max_array_size ) const;

  void applyNameTransform(const std::string& msg_identifier,
                          const FlatMessage& container,
                          RenamedValues* renamed_value , bool dont_add_topicname = false);

  typedef std::function<void(const ROSType&, Span<uint8_t>&)> VisitingCallback;

  void applyVisitorToBuffer(const std::string& msg_identifier, const ROSType &monitored_type,
                            Span<uint8_t> &buffer,
                            VisitingCallback callback) const;

  template <typename T>
  T extractField(const std::string& msg_identifier, const Span<uint8_t> &buffer);


  void setWarningsStream(std::ostream* output) { _global_warnings = output; }

private:


  struct RulesCache{
    RulesCache( const SubstitutionRule& r):
      rule( &r ), pattern_head(nullptr), alias_head(nullptr)
    {}
    const SubstitutionRule* rule;
    const StringTreeNode* pattern_head;
    const StringTreeNode* alias_head;
    bool operator==(const RulesCache& other) { return  this->rule == other.rule; }
  };

  std::unordered_map<std::string, ROSMessageInfo> _registered_messages;
  std::unordered_map<ROSType,     std::unordered_set<SubstitutionRule>>   _registered_rules;
  std::unordered_map<std::string, std::vector<RulesCache>>  _rule_caches;

  void updateRuleCache();

  bool _rule_cache_dirty;

  void createTrees(ROSMessageInfo &info, const std::string &type_name) const;

  std::ostream* _global_warnings;

  std::vector<int> _alias_array_pos;
  std::vector<std::string> _formatted_string;
  std::vector<int8_t> _substituted;
  MaxArrayPolicy _discard_large_array;
  BlobPolicy _blob_policy;
};

//---------------------------------------------------

template<typename T> inline
T Parser::extractField(const std::string &msg_identifier,
                       const Span<uint8_t> &buffer)
{
    T out;
    bool found = false;

    const ROSMessageInfo* msg_info = getMessageInfo(msg_identifier);
    if( msg_info == nullptr)
    {
      throw std::runtime_error("extractField: msg_identifier not registered. Use registerMessageDefinition" );
    }

    const ROSType monitored_type (ros::message_traits::DataType<T>::value());

    if( getMessageByType( monitored_type, *msg_info) == nullptr)
    {
        throw std::runtime_error("extractField: message type doesn't contain this field type" );
    }

    std::function<void(const MessageTreeNode*)> recursiveImpl;
    size_t buffer_offset = 0;

    recursiveImpl = [&](const MessageTreeNode* msg_node)
    {
      if( found ) return;

      const ROSMessage* msg_definition = msg_node->value();
      const ROSType& msg_type = msg_definition->type();

      size_t index_m = 0;

      if( msg_type == monitored_type  )
      {
            ros::serialization::IStream is( buffer.data() + buffer_offset,
                                            buffer.size() - buffer_offset );
            ros::serialization::deserialize(is, out);
            found = true;
            return;
      }
       // subfields
      for (const ROSField& field : msg_definition->fields() )
      {
        if(field.isConstant() ) continue;

        const ROSType& field_type = field.type();

        int32_t array_size = field.arraySize();
        if( array_size == -1)
        {
          ReadFromBuffer( buffer, buffer_offset, array_size );
        }
        //------------------------------------
        if( field_type.isBuiltin() && field_type != monitored_type )
        {
            //fast skip
            if( field_type.typeSize() >= 1 )
            {
                buffer_offset += field_type.typeSize() * array_size;
            }
            else{
                ReadFromBufferToVariant( field_type.typeID(), buffer, buffer_offset );
            }
        }
        else
        {
          for (int i=0; i<array_size; i++ )
          {
            recursiveImpl( msg_node->child(index_m) );
            if( found ) return;
          }
          index_m++;
        }
      } // end for fields

    }; //end lambda

    //start recursion
    recursiveImpl( msg_info->message_tree.croot() );

    return out;
}



}

#endif // ROS_INTROSPECTION_HPP