Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef ROS_INTROSPECTION_HPP
00036 #define ROS_INTROSPECTION_HPP
00037
00038 #include <unordered_set>
00039 #include "ros_type_introspection/stringtree_leaf.hpp"
00040 #include "ros_type_introspection/substitution_rule.hpp"
00041 #include "ros_type_introspection/helper_functions.hpp"
00042 #include "absl/types/span.h"
00043
00044 namespace RosIntrospection{
00045
00046 struct FlatMessage {
00047
00049 const StringTree* tree;
00050
00053 std::vector< std::pair<StringTreeLeaf, Variant> > value;
00054
00057 std::vector< std::pair<StringTreeLeaf, std::string> > name;
00058
00062 std::vector< std::pair<StringTreeLeaf, std::vector<uint8_t>>> blob;
00063 };
00064
00065 typedef std::vector< std::pair<std::string, Variant> > RenamedValues;
00066
00067 class Parser{
00068
00069 public:
00070 Parser(): _rule_cache_dirty(true), _global_warnings(&std::cerr), _discard_large_array(true) {}
00071
00072 void setMaxArrayPolicy( bool discard_entire_array )
00073 {
00074 _discard_large_array = discard_entire_array;
00075 }
00076
00090 void registerMessageDefinition(const std::string& message_identifier,
00091 const ROSType &main_type,
00092 const std::string& definition);
00093
00100 void registerRenamingRules(const ROSType& type,
00101 const std::vector<SubstitutionRule> &rules );
00102
00109 const ROSMessageInfo* getMessageInfo(const std::string& msg_identifier) const;
00110
00118 const ROSMessage *getMessageByType(const ROSType& type, const ROSMessageInfo &info) const;
00119
00143 bool deserializeIntoFlatContainer(const std::string& msg_identifier,
00144 absl::Span<uint8_t> buffer,
00145 FlatMessage* flat_container_output,
00146 const uint32_t max_array_size ) const;
00147
00168 void applyNameTransform(const std::string& msg_identifier,
00169 const FlatMessage& container,
00170 RenamedValues* renamed_value );
00171
00172 typedef std::function<void(const ROSType&, absl::Span<uint8_t>&)> VisitingCallback;
00173
00186 void applyVisitorToBuffer(const std::string& msg_identifier, const ROSType &monitored_type,
00187 absl::Span<uint8_t> &buffer,
00188 VisitingCallback callback) const;
00189
00190 template <typename T>
00191 T extractField(const std::string& msg_identifier, const absl::Span<uint8_t> &buffer);
00192
00193
00195 void setWarningsStream(std::ostream* output) { _global_warnings = output; }
00196
00197 private:
00198
00199
00200 struct RulesCache{
00201 RulesCache( const SubstitutionRule& r):
00202 rule( &r ), pattern_head(nullptr), alias_head(nullptr)
00203 {}
00204 const SubstitutionRule* rule;
00205 const StringTreeNode* pattern_head;
00206 const StringTreeNode* alias_head;
00207 bool operator==(const RulesCache& other) { return this->rule == other.rule; }
00208 };
00209
00210 std::unordered_map<std::string, ROSMessageInfo> _registered_messages;
00211 std::unordered_map<ROSType, std::unordered_set<SubstitutionRule>> _registered_rules;
00212 std::unordered_map<std::string, std::vector<RulesCache>> _rule_caches;
00213
00214 void updateRuleCache();
00215
00216 bool _rule_cache_dirty;
00217
00218 void createTrees(ROSMessageInfo &info, const std::string &type_name) const;
00219
00220 std::ostream* _global_warnings;
00221
00222 std::vector<int> _alias_array_pos;
00223 std::vector<std::string> _formatted_string;
00224 std::vector<int8_t> _substituted;
00225 bool _discard_large_array;
00226 };
00227
00228
00229
00230 template<typename T> inline
00231 T Parser::extractField(const std::string &msg_identifier,
00232 const absl::Span<uint8_t> &buffer)
00233 {
00234 T out;
00235 bool found = false;
00236
00237 const ROSMessageInfo* msg_info = getMessageInfo(msg_identifier);
00238 if( msg_info == nullptr)
00239 {
00240 throw std::runtime_error("extractField: msg_identifier not registered. Use registerMessageDefinition" );
00241 }
00242
00243 const ROSType monitored_type (ros::message_traits::DataType<T>::value());
00244
00245 if( getMessageByType( monitored_type, *msg_info) == nullptr)
00246 {
00247 throw std::runtime_error("extractField: message type doesn't contain this field type" );
00248 }
00249
00250 std::function<void(const MessageTreeNode*)> recursiveImpl;
00251 size_t buffer_offset = 0;
00252
00253 recursiveImpl = [&](const MessageTreeNode* msg_node)
00254 {
00255 if( found ) return;
00256
00257 const ROSMessage* msg_definition = msg_node->value();
00258 const ROSType& msg_type = msg_definition->type();
00259
00260 size_t index_m = 0;
00261
00262 if( msg_type == monitored_type )
00263 {
00264 ros::serialization::IStream is( buffer.data() + buffer_offset,
00265 buffer.size() - buffer_offset );
00266 ros::serialization::deserialize(is, out);
00267 found = true;
00268 return;
00269 }
00270
00271 for (const ROSField& field : msg_definition->fields() )
00272 {
00273 if(field.isConstant() ) continue;
00274
00275 const ROSType& field_type = field.type();
00276
00277 int32_t array_size = field.arraySize();
00278 if( array_size == -1)
00279 {
00280 ReadFromBuffer( buffer, buffer_offset, array_size );
00281 }
00282
00283 if( field_type.isBuiltin() && field_type != monitored_type )
00284 {
00285
00286 if( field_type.typeSize() >= 1 )
00287 {
00288 buffer_offset += field_type.typeSize() * array_size;
00289 }
00290 else{
00291 ReadFromBufferToVariant( field_type.typeID(), buffer, buffer_offset );
00292 }
00293 }
00294 else
00295 {
00296 for (int i=0; i<array_size; i++ )
00297 {
00298 recursiveImpl( msg_node->child(index_m) );
00299 if( found ) return;
00300 }
00301 index_m++;
00302 }
00303 }
00304
00305 };
00306
00307
00308 recursiveImpl( msg_info->message_tree.croot() );
00309
00310 return out;
00311 }
00312
00313
00314
00315 }
00316
00317 #endif // ROS_INTROSPECTION_HPP