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
00036 #include <boost/algorithm/string.hpp>
00037 #include <boost/utility/string_ref.hpp>
00038 #include <boost/lexical_cast.hpp>
00039 #include <boost/regex.hpp>
00040 #include <boost/algorithm/string/regex.hpp>
00041 #include <functional>
00042 #include "ros_type_introspection/ros_introspection.hpp"
00043 #include "ros_type_introspection/helper_functions.hpp"
00044
00045 namespace RosIntrospection {
00046
00047 void Parser::createTrees(ROSMessageInfo& info, const std::string &type_name) const
00048 {
00049 std::function<void(const ROSMessage*, StringTreeNode*, MessageTreeNode* )> recursiveTreeCreator;
00050
00051 recursiveTreeCreator = [&](const ROSMessage* msg_definition, StringTreeNode* string_node, MessageTreeNode* msg_node)
00052 {
00053
00054 const size_t NUM_FIELDS = msg_definition->fields().size();
00055
00056 string_node->children().reserve( NUM_FIELDS );
00057 msg_node->children().reserve( NUM_FIELDS );
00058
00059 for (const ROSField& field : msg_definition->fields() )
00060 {
00061 if(field.isConstant() == false) {
00062
00063
00064 string_node->addChild( field.name() );
00065 StringTreeNode* new_string_node = &(string_node->children().back());
00066 if( field.isArray())
00067 {
00068 new_string_node->children().reserve(1);
00069 new_string_node = new_string_node->addChild("#");
00070 }
00071
00072 const ROSMessage* next_msg = nullptr;
00073
00074 if( field.type().isBuiltin() == false)
00075 {
00076 next_msg = getMessageByType( field.type(), info );
00077 if( next_msg == nullptr)
00078 {
00079 throw std::runtime_error("This type was not registered " );
00080 }
00081 msg_node->addChild( next_msg );
00082 MessageTreeNode* new_msg_node = &(msg_node->children().back());
00083 recursiveTreeCreator(next_msg, new_string_node, new_msg_node);
00084 }
00085 }
00086 }
00087 };
00088
00089 info.string_tree.root()->setValue( type_name );
00090 info.message_tree.root()->setValue( &info.type_list.front() );
00091
00092
00093 recursiveTreeCreator( &info.type_list.front(),
00094 info.string_tree.root(),
00095 info.message_tree.root());
00096 }
00097
00098 inline bool operator ==( const std::string& a, const absl::string_view& b)
00099 {
00100 return ( a.size() == b.size() && std::strncmp( a.data(), b.data(), a.size()) == 0);
00101 }
00102
00103 inline bool FindPattern(const std::vector<absl::string_view> &pattern,
00104 size_t index, const StringTreeNode *tail,
00105 const StringTreeNode **head)
00106 {
00107 if( tail->value() == pattern[index] )
00108 {
00109 index++;
00110 }
00111 else{
00112 if( index > 0 ){
00113
00114 FindPattern( pattern, 0, tail, head);
00115 return false;
00116 }
00117 index = 0;
00118 }
00119
00120 if( index == pattern.size()){
00121 *head = ( tail );
00122 return true;
00123 }
00124
00125 bool found = false;
00126
00127 for (auto& child: tail->children() ) {
00128
00129 found = FindPattern( pattern, index, &child, head);
00130 if( found) break;
00131 }
00132 return found;
00133 }
00134
00135
00136 void Parser::registerRenamingRules(const ROSType &type, const std::vector<SubstitutionRule> &given_rules)
00137 {
00138 std::unordered_set<SubstitutionRule>& rule_set = _registered_rules[type];
00139 for(const auto& rule: given_rules)
00140 {
00141 if( rule_set.find( rule ) == rule_set.end() )
00142 {
00143 rule_set.insert( rule );
00144 _rule_cache_dirty = true;
00145 }
00146 }
00147 }
00148
00149 void Parser::updateRuleCache()
00150 {
00151 if( _rule_cache_dirty == false)
00152 {
00153 return;
00154 }
00155 else{
00156 _rule_cache_dirty = false;
00157 }
00158 for(const auto& rule_it: _registered_rules )
00159 {
00160 const ROSType& type = rule_it.first;
00161 const std::unordered_set<SubstitutionRule>& rule_set = rule_it.second;
00162
00163 for(const auto& msg_it: _registered_messages)
00164 {
00165 const std::string& msg_identifier = msg_it.first;
00166 const ROSMessageInfo& msg_info = msg_it.second;
00167
00168 if( getMessageByType(type, msg_info) )
00169 {
00170 std::vector<RulesCache>& cache_vector = _rule_caches[msg_identifier];
00171 for(const auto& rule: rule_set )
00172 {
00173 RulesCache cache(rule);
00174 FindPattern( cache.rule->pattern(), 0, msg_info.string_tree.croot(), &cache.pattern_head );
00175 FindPattern( cache.rule->alias(), 0, msg_info.string_tree.croot(), &cache.alias_head );
00176 if( cache.pattern_head && cache.alias_head
00177 && std::find( cache_vector.begin(), cache_vector.end(), cache) == cache_vector.end()
00178 )
00179 {
00180 cache_vector.push_back( std::move(cache) );
00181 }
00182 }
00183 }
00184 }
00185 }
00186 }
00187
00188
00189
00190 void Parser::registerMessageDefinition(const std::string &msg_definition,
00191 const ROSType &main_type,
00192 const std::string &definition)
00193 {
00194 if( _registered_messages.count(msg_definition) > 0 )
00195 {
00196 return;
00197 }
00198 _rule_cache_dirty = true;
00199
00200 const boost::regex msg_separation_regex("^=+\\n+");
00201
00202 std::vector<std::string> split;
00203 std::vector<const ROSType*> all_types;
00204
00205 boost::split_regex(split, definition, msg_separation_regex);
00206
00207 ROSMessageInfo info;
00208 info.type_list.reserve( split.size() );
00209
00210 for (size_t i = 0; i < split.size(); ++i)
00211 {
00212 ROSMessage msg( split[i] );
00213 if( i == 0)
00214 {
00215 msg.mutateType( main_type );
00216 }
00217
00218 info.type_list.push_back( std::move(msg) );
00219 all_types.push_back( &(info.type_list.back().type()) );
00220 }
00221
00222 for( ROSMessage& msg: info.type_list )
00223 {
00224 msg.updateMissingPkgNames( all_types );
00225 }
00226
00227
00228 createTrees(info, msg_definition);
00229
00230
00231
00232 _registered_messages.insert( std::make_pair(msg_definition, std::move(info) ) );
00233 }
00234
00235 const ROSMessageInfo *Parser::getMessageInfo(const std::string &msg_identifier) const
00236 {
00237 auto it = _registered_messages.find(msg_identifier);
00238 if( it != _registered_messages.end() )
00239 {
00240 return &(it->second);
00241 }
00242 return nullptr;
00243 }
00244
00245 const ROSMessage* Parser::getMessageByType(const ROSType &type, const ROSMessageInfo& info) const
00246 {
00247 for(const ROSMessage& msg: info.type_list)
00248 {
00249 if( msg.type() == type )
00250 {
00251 return &msg;
00252 }
00253 }
00254 return nullptr;
00255 }
00256
00257 void Parser::applyVisitorToBuffer(const std::string &msg_identifier,
00258 const ROSType& monitored_type,
00259 absl::Span<uint8_t> &buffer,
00260 Parser::VisitingCallback callback) const
00261 {
00262 const ROSMessageInfo* msg_info = getMessageInfo(msg_identifier);
00263
00264 if( msg_info == nullptr)
00265 {
00266 throw std::runtime_error("extractField: msg_identifier not registered. Use registerMessageDefinition" );
00267 }
00268 if( getMessageByType( monitored_type, *msg_info) == nullptr)
00269 {
00270
00271 return;
00272 }
00273
00274 std::function<void(const MessageTreeNode*)> recursiveImpl;
00275 size_t buffer_offset = 0;
00276
00277 recursiveImpl = [&](const MessageTreeNode* msg_node)
00278 {
00279 const ROSMessage* msg_definition = msg_node->value();
00280 const ROSType& msg_type = msg_definition->type();
00281
00282 const bool matching = ( msg_type == monitored_type );
00283
00284 uint8_t* prev_buffer_ptr = buffer.data() + buffer_offset;
00285 size_t prev_offset = buffer_offset;
00286
00287 size_t index_m = 0;
00288
00289 for (const ROSField& field : msg_definition->fields() )
00290 {
00291 if(field.isConstant() ) continue;
00292
00293 const ROSType& field_type = field.type();
00294
00295 int32_t array_size = field.arraySize();
00296 if( array_size == -1)
00297 {
00298 ReadFromBuffer( buffer, buffer_offset, array_size );
00299 }
00300
00301
00302
00303 if( field_type.isBuiltin() )
00304 {
00305 for (int i=0; i<array_size; i++ )
00306 {
00307
00308 ReadFromBufferToVariant( field_type.typeID(), buffer, buffer_offset );
00309 }
00310 }
00311 else{
00312
00313 for (int i=0; i<array_size; i++ )
00314 {
00315 recursiveImpl( msg_node->child(index_m) );
00316 }
00317 index_m++;
00318 }
00319 }
00320 if( matching )
00321 {
00322 absl::Span<uint8_t> view( prev_buffer_ptr, buffer_offset - prev_offset);
00323 callback( monitored_type, view );
00324 }
00325 };
00326
00327
00328 recursiveImpl( msg_info->message_tree.croot() );
00329 }
00330
00331 bool Parser::deserializeIntoFlatContainer(const std::string& msg_identifier,
00332 absl::Span<uint8_t> buffer,
00333 FlatMessage* flat_container,
00334 const uint32_t max_array_size ) const
00335 {
00336
00337 bool entire_message_parse = true;
00338 const ROSMessageInfo* msg_info = getMessageInfo(msg_identifier);
00339
00340 size_t value_index = 0;
00341 size_t name_index = 0;
00342 size_t blob_index = 0;
00343
00344 if( msg_info == nullptr)
00345 {
00346 throw std::runtime_error("deserializeIntoFlatContainer: msg_identifier not registerd. Use registerMessageDefinition" );
00347 }
00348 size_t buffer_offset = 0;
00349
00350 std::function<void(const MessageTreeNode*,StringTreeLeaf,bool)> deserializeImpl;
00351
00352 deserializeImpl = [&](
00353 const MessageTreeNode* msg_node,
00354 const StringTreeLeaf& tree_leaf,
00355 bool DO_STORE)
00356 {
00357 const ROSMessage* msg_definition = msg_node->value();
00358 size_t index_s = 0;
00359 size_t index_m = 0;
00360
00361 for (const ROSField& field : msg_definition->fields() )
00362 {
00363 if(field.isConstant() ) continue;
00364
00365 const ROSType& field_type = field.type();
00366
00367 auto new_tree_leaf = tree_leaf;
00368 new_tree_leaf.node_ptr = tree_leaf.node_ptr->child(index_s);
00369
00370 int32_t array_size = field.arraySize();
00371 if( array_size == -1)
00372 {
00373 ReadFromBuffer( buffer, buffer_offset, array_size );
00374 }
00375 if( field.isArray())
00376 {
00377 new_tree_leaf.index_array.push_back(0);
00378 new_tree_leaf.node_ptr = new_tree_leaf.node_ptr->child(0);
00379 }
00380
00381 bool IS_BLOB = false;
00382
00383
00384 if( array_size > static_cast<int32_t>(max_array_size))
00385 {
00386 if( builtinSize(field_type.typeID()) == 1){
00387 IS_BLOB = true;
00388 }
00389 else{
00390 if( _discard_large_array ){
00391 DO_STORE = false;
00392 }
00393 entire_message_parse = false;
00394 }
00395 }
00396
00397 if( IS_BLOB )
00398 {
00399 if( flat_container->blob.size() <= blob_index)
00400 {
00401 const size_t increased_size = std::max( size_t(32), flat_container->blob.size() * 3/2);
00402 flat_container->blob.resize( increased_size );
00403 }
00404 if( buffer_offset + array_size > buffer.size() )
00405 {
00406 throw std::runtime_error("Buffer overrun in deserializeIntoFlatContainer (blob)");
00407 }
00408 if( DO_STORE )
00409 {
00410 flat_container->blob[blob_index].first = new_tree_leaf ;
00411 std::vector<uint8_t>& blob = flat_container->blob[blob_index].second;
00412 blob_index++;
00413 blob.resize( array_size );
00414 std::memcpy( blob.data(), &buffer[buffer_offset], array_size);
00415 }
00416 buffer_offset += array_size;
00417 }
00418 else
00419 {
00420 bool DO_STORE_ARRAY = DO_STORE;
00421 for (int i=0; i<array_size; i++ )
00422 {
00423 if( DO_STORE_ARRAY && i >= max_array_size )
00424 {
00425 DO_STORE_ARRAY = false;
00426 }
00427
00428 if( field.isArray() && DO_STORE_ARRAY )
00429 {
00430 new_tree_leaf.index_array.back() = i;
00431 }
00432
00433 if( field_type.typeID() == STRING )
00434 {
00435 if( flat_container->name.size() <= name_index)
00436 {
00437 const size_t increased_size = std::max( size_t(32), flat_container->name.size() * 3/2);
00438 flat_container->name.resize( increased_size );
00439 }
00440 std::string& name = flat_container->name[name_index].second;
00441 ReadFromBuffer<std::string>( buffer, buffer_offset, name );
00442
00443 if( DO_STORE_ARRAY )
00444 {
00445 flat_container->name[name_index].first = new_tree_leaf ;
00446 name_index++;
00447 }
00448 }
00449 else if( field_type.isBuiltin() )
00450 {
00451 if( flat_container->value.size() <= value_index)
00452 {
00453 const size_t increased_size = std::max( size_t(32), flat_container->value.size() * 3/2);
00454 flat_container->value.resize( increased_size );
00455 }
00456
00457 Variant var = ReadFromBufferToVariant( field_type.typeID(),
00458 buffer,
00459 buffer_offset );
00460 if( DO_STORE_ARRAY )
00461 {
00462 flat_container->value[value_index] = std::make_pair( new_tree_leaf, std::move(var) );
00463 value_index++;
00464 }
00465 }
00466 else{
00467
00468 deserializeImpl(msg_node->child(index_m),
00469 new_tree_leaf,
00470 DO_STORE_ARRAY);
00471 }
00472 }
00473 }
00474
00475 if( field_type.typeID() == OTHER )
00476 {
00477 index_m++;
00478 }
00479 index_s++;
00480 }
00481 };
00482
00483
00484 flat_container->tree = &msg_info->string_tree;
00485
00486 StringTreeLeaf rootnode;
00487 rootnode.node_ptr = msg_info->string_tree.croot();
00488
00489 deserializeImpl( msg_info->message_tree.croot(),
00490 rootnode,
00491 true);
00492
00493 flat_container->name.resize( name_index );
00494 flat_container->value.resize( value_index );
00495 flat_container->blob.resize( blob_index );
00496
00497 if( buffer_offset != buffer.size() )
00498 {
00499 throw std::runtime_error("buildRosFlatType: There was an error parsing the buffer" );
00500 }
00501 return entire_message_parse;
00502 }
00503
00504 inline bool isNumberPlaceholder( const absl::string_view& s)
00505 {
00506 return s.size() == 1 && s[0] == '#';
00507 }
00508
00509 inline bool isSubstitutionPlaceholder( const absl::string_view& s)
00510 {
00511 return s.size() == 1 && s[0] == '@';
00512 }
00513
00514
00515
00516 inline int PatternMatchAndIndexPosition(const StringTreeLeaf& leaf,
00517 const StringTreeNode* pattern_head )
00518 {
00519 const StringTreeNode* node_ptr = leaf.node_ptr;
00520
00521 int pos = leaf.index_array.size() -1;
00522
00523 while( node_ptr )
00524 {
00525 if( node_ptr != pattern_head )
00526 {
00527 if( isNumberPlaceholder( node_ptr->value() ))
00528 {
00529 pos--;
00530 }
00531 }
00532 else{
00533 return pos;
00534 }
00535 node_ptr = node_ptr->parent();
00536 }
00537 return -1;
00538 }
00539
00540 template <typename VectorType>
00541 inline void JoinStrings( const VectorType& vect, const char separator, std::string& destination)
00542 {
00543 size_t count = 0;
00544 for(const auto &v: vect ) count += v.size();
00545
00546
00547
00548
00549 destination.resize( count + vect.size() -1 );
00550
00551 char* buffer = &destination[0];
00552 size_t buff_pos = 0;
00553
00554 for (size_t c = 0; c < vect.size()-1; c++)
00555 {
00556 const size_t S = vect[c].size();
00557 std::memcpy( &buffer[buff_pos], vect[c].data(), S );
00558 buff_pos += S;
00559 buffer[buff_pos++] = separator;
00560 }
00561 std::memcpy( &buffer[buff_pos], vect.back().data(), vect.back().size() );
00562 }
00563
00564 void Parser::applyNameTransform(const std::string& msg_identifier,
00565 const FlatMessage& container,
00566 RenamedValues *renamed_value )
00567 {
00568 if( _rule_cache_dirty )
00569 {
00570 updateRuleCache();
00571 }
00572 auto rule_found = _rule_caches.find(msg_identifier);
00573
00574 const size_t num_values = container.value.size();
00575 const size_t num_names = container.name.size();
00576
00577 renamed_value->resize( container.value.size() );
00578
00579
00580 _alias_array_pos.resize( num_names );
00581 _formatted_string.reserve( num_values );
00582 _formatted_string.clear();
00583
00584 _substituted.resize( num_values );
00585 for(size_t i=0; i<num_values; i++) { _substituted[i] = false; }
00586
00587
00588
00589 if( rule_found != _rule_caches.end() )
00590 {
00591 const std::vector<RulesCache>& rules_cache = rule_found->second;
00592
00593 for(const RulesCache& cache: rules_cache)
00594 {
00595 const SubstitutionRule* rule = cache.rule;
00596 const StringTreeNode* pattern_head = cache.pattern_head;
00597 const StringTreeNode* alias_head = cache.alias_head;
00598
00599 if( !pattern_head || !alias_head ) continue;
00600
00601 for (size_t n=0; n<num_names; n++)
00602 {
00603 const StringTreeLeaf& name_leaf = container.name[n].first;
00604 _alias_array_pos[n] = PatternMatchAndIndexPosition(name_leaf, alias_head);
00605 }
00606
00607 for(size_t value_index = 0; value_index<num_values; value_index++)
00608 {
00609 if( _substituted[value_index]) continue;
00610
00611 const auto& value_leaf = container.value[value_index];
00612
00613 const StringTreeLeaf& leaf = value_leaf.first;
00614
00615 int pattern_array_pos = PatternMatchAndIndexPosition(leaf, pattern_head);
00616
00617 if( pattern_array_pos>= 0)
00618 {
00619 const std::string* new_name = nullptr;
00620
00621 for (size_t n=0; n < num_names; n++)
00622 {
00623 const auto & it = container.name[n];
00624 const StringTreeLeaf& alias_leaf = it.first;
00625
00626 if( _alias_array_pos[n] >= 0 )
00627 {
00628 if( alias_leaf.index_array[ _alias_array_pos[n] ] ==
00629 leaf.index_array[ pattern_array_pos] )
00630 {
00631 new_name = &(it.second);
00632 break;
00633 }
00634 }
00635 }
00636
00637
00638 if( new_name )
00639 {
00640 absl::InlinedVector<absl::string_view, 12> concatenated_name;
00641
00642 const StringTreeNode* node_ptr = leaf.node_ptr;
00643
00644 int position = leaf.index_array.size()-1;
00645
00646 while( node_ptr != pattern_head)
00647 {
00648 const absl::string_view& str_val = node_ptr->value();
00649
00650 if( isNumberPlaceholder( str_val ) )
00651 {
00652 char buffer[16];
00653 const int number = leaf.index_array[position--];
00654 int str_size = print_number( buffer, number );
00655 _formatted_string.push_back( std::string(buffer, str_size) );
00656 concatenated_name.push_back( _formatted_string.back() );
00657 }
00658 else{
00659 concatenated_name.push_back( str_val );
00660 }
00661 node_ptr = node_ptr->parent();
00662 }
00663
00664 for (int s = rule->substitution().size()-1; s >= 0; s--)
00665 {
00666 const absl::string_view& str_val = rule->substitution()[s];
00667
00668 if( isSubstitutionPlaceholder(str_val) )
00669 {
00670 concatenated_name.push_back( *new_name );
00671 position--;
00672 }
00673 else{ concatenated_name.push_back( str_val ); }
00674 }
00675
00676 for (size_t p = 0; p < rule->pattern().size() && node_ptr; p++)
00677 {
00678 node_ptr = node_ptr->parent();
00679 }
00680
00681 while( node_ptr )
00682 {
00683 absl::string_view str_val = node_ptr->value();
00684
00685 if( isNumberPlaceholder(str_val) )
00686 {
00687 char buffer[16];
00688 const int number = leaf.index_array[position--];
00689 int str_size = print_number( buffer, number );
00690 _formatted_string.push_back( std::string(buffer, str_size) );
00691 concatenated_name.push_back( _formatted_string.back() );
00692 }
00693 else{
00694 concatenated_name.push_back( str_val );
00695 }
00696 node_ptr = node_ptr->parent();
00697 }
00698
00699
00700 auto& renamed_pair = (*renamed_value)[value_index];
00701
00702 std::reverse(concatenated_name.begin(), concatenated_name.end());
00703 JoinStrings( concatenated_name, '/', renamed_pair.first);
00704 renamed_pair.second = value_leaf.second ;
00705
00706 _substituted[value_index] = true;
00707
00708 }
00709 }
00710 }
00711 }
00712 }
00713
00714 for(size_t value_index=0; value_index< container.value.size(); value_index++)
00715 {
00716 if( !_substituted[value_index] )
00717 {
00718 const std::pair<StringTreeLeaf, Variant> & value_leaf = container.value[value_index];
00719
00720 std::string& destination = (*renamed_value)[value_index].first;
00721 value_leaf.first.toStr( destination );
00722 (*renamed_value)[value_index].second = value_leaf.second ;
00723 }
00724 }
00725 }
00726
00727 }
00728
00729