renamer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright 2016 Davide Faconti
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of Willow Garage, Inc. nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 ********************************************************************/
00034 
00035 
00036 #include <boost/algorithm/string.hpp>
00037 #include <boost/utility/string_ref.hpp>
00038 #include "ros_type_introspection/renamer.hpp"
00039 
00040 namespace RosIntrospection{
00041 
00042 inline bool isNumberPlaceholder( const SString& s)
00043 {
00044   return s.size() == 1 && s.at(0) == '#';
00045 }
00046 
00047 inline bool isSubstitutionPlaceholder( const SString& s)
00048 {
00049   return s.size() == 1 && s.at(0) == '@';
00050 }
00051 
00052 inline bool FindPattern( const std::vector<SString>& pattern,  size_t index,
00053                          const StringTreeNode* tail,
00054                          const  StringTreeNode** head )
00055 {
00056   if(  tail->value()  == pattern[index])
00057   {
00058     index++;
00059   }
00060   else{ // mismatch
00061     if( index > 0 ){
00062       // reset counter;
00063       FindPattern( pattern, 0, tail, head);
00064       return false;
00065     }
00066     index = 0;
00067   }
00068 
00069   if( index == pattern.size()){
00070     *head = ( tail );
00071     return true;
00072   }
00073 
00074   bool found = false;
00075 
00076   for (auto& child: tail->children() ) {
00077 
00078     found = FindPattern( pattern, index, &child, head);
00079     if( found) break;
00080   }
00081   return found;
00082 }
00083 
00084 
00085 // given a leaf of the tree, that can have multiple index_array,
00086 // find the only index which corresponds to the # in the pattern
00087 int  PatternMatchAndIndexPosition(const StringTreeLeaf& leaf,
00088                                   const StringTreeNode* pattern_head )
00089 {
00090   const StringTreeNode* node_ptr = leaf.node_ptr;
00091 
00092   int pos = leaf.array_size-1;
00093 
00094   while( node_ptr )
00095   {
00096     if( node_ptr != pattern_head )
00097     {
00098       if( isNumberPlaceholder( node_ptr->value() ))
00099       {
00100         pos--;
00101       }
00102     }
00103     else{
00104       return pos;
00105     }
00106     node_ptr = node_ptr->parent();
00107   } // end while
00108   return -1;
00109 }
00110 
00111 
00112 void applyNameTransform(const std::vector<SubstitutionRule> &rules,
00113                         const ROSTypeFlat& container,
00114                         RenamedValues& renamed_value )
00115 {
00116 
00117   const bool debug = false ;
00118 
00119   if( debug) std::cout << container.tree << std::endl;
00120 
00121   renamed_value.resize( container.value.size() );
00122 
00123   std::vector<uint8_t> substituted( container.value.size() );
00124   for(auto& sub: substituted) { sub = false; }
00125 
00126   size_t renamed_index = 0;
00127 
00128   std::vector<int> alias_array_pos( container.name.size() );
00129   std::vector<SString> formatted_string;
00130   formatted_string.reserve(20);
00131 
00132   for(const auto& rule: rules)
00133   {
00134     const StringTreeNode* pattern_head = nullptr;
00135     const StringTreeNode* alias_head = nullptr;
00136 
00137     FindPattern( rule.pattern(), 0, container.tree.croot(), &pattern_head );
00138     if( !pattern_head) continue;
00139 
00140     FindPattern( rule.alias(),   0, container.tree.croot(), &alias_head );
00141     if(!alias_head) continue;
00142 
00143     for (int n=0; n< container.name.size(); n++)
00144     {
00145       const StringTreeLeaf& alias_leaf = container.name[n].first;
00146       alias_array_pos[n] = PatternMatchAndIndexPosition(alias_leaf, alias_head);
00147     }
00148 
00149     for(size_t i=0; i< container.value.size(); i++)
00150     {
00151       if( substituted[i]) continue;
00152 
00153       const auto& value_leaf = container.value[i];
00154 
00155       const StringTreeLeaf& leaf = value_leaf.first;
00156 
00157       int pattern_array_pos = PatternMatchAndIndexPosition(leaf,   pattern_head);
00158 
00159       if( pattern_array_pos>= 0) // -1 if pattern doesn't match
00160       {
00161         if( debug) std::cout << " match pattern... ";
00162 
00163         const SString* new_name = nullptr;
00164 
00165         for (int n=0; n< container.name.size(); n++)
00166         {
00167           const auto & it = container.name[n];
00168           const StringTreeLeaf& alias_leaf = it.first;
00169 
00170           if( alias_array_pos[n] >= 0 ) // -1 if pattern doesn't match
00171           {
00172             if( alias_leaf.index_array[ alias_array_pos[n] ] ==
00173                 leaf.index_array[ pattern_array_pos] )
00174             {
00175               if( debug) std::cout << " substitution: " << it.second << std::endl;
00176               new_name =  &(it.second);
00177               break;
00178             }
00179           }
00180         }
00181 
00182         //--------------------------
00183         if( new_name )
00184         {
00185           int char_count = 0;
00186           std::vector<const SString*> concatenated_name;
00187           concatenated_name.reserve( 10 );
00188 
00189           const StringTreeNode* node_ptr = leaf.node_ptr;
00190 
00191           int position = leaf.array_size - 1;
00192 
00193           while( node_ptr != pattern_head)
00194           {
00195             const SString* value = &node_ptr->value();
00196 
00197             if( isNumberPlaceholder( *value) ){
00198               char buffer[16];
00199               print_number( buffer, leaf.index_array[position--] );
00200               formatted_string.push_back( std::move(SString(buffer)) );
00201               value = &formatted_string.back();
00202             }
00203 
00204             char_count += value->size();
00205             if( debug) std::cout << "A: " << *value << std::endl;;
00206             concatenated_name.push_back( std::move(value) );
00207             node_ptr = node_ptr->parent();
00208           }
00209 
00210           for (int s = rule.substitution().size()-1; s >= 0; s--)
00211           {
00212             const SString* value = &rule.substitution()[s];
00213 
00214             if( isSubstitutionPlaceholder( *value) ) {
00215               value = new_name;
00216               position--;
00217             }
00218 
00219             char_count += value->size();
00220             if( debug) std::cout << "B: " << *value << std::endl;;
00221             concatenated_name.push_back( std::move(value) );
00222           }
00223 
00224           for (int p = 0; p < rule.pattern().size() && node_ptr; p++)
00225           {
00226             node_ptr = node_ptr->parent();
00227           }
00228 
00229           while( node_ptr )
00230           {
00231             const SString* value = &node_ptr->value();
00232 
00233             if( isNumberPlaceholder( *value) ){
00234               char buffer[16];
00235               print_number( buffer, leaf.index_array[position--] );
00236               formatted_string.push_back( std::move(SString(buffer)) );
00237               value = &formatted_string.back();
00238             }
00239 
00240             char_count += value->size();
00241             if( debug) std::cout << "C: " << *value << std::endl;;
00242             concatenated_name.push_back( std::move(value) );
00243             node_ptr = node_ptr->parent();
00244           }
00245 
00246           //------------------------
00247           std::string new_identifier;
00248           new_identifier.reserve(80);
00249 
00250           for (int c = concatenated_name.size()-1; c >= 0; c--)
00251           {
00252             new_identifier += ( concatenated_name[c]->data() );
00253             if( c>0 ) new_identifier.append("/");
00254           }
00255           if( debug) std::cout << "Result: " << new_identifier << std::endl;
00256 
00257           renamed_value[renamed_index].first   = std::move( new_identifier );
00258           renamed_value[renamed_index].second  = value_leaf.second ;
00259           renamed_index++;
00260           substituted[i] = true;
00261           if( debug) std::cout << std::endl;
00262         }// end if( new_name )
00263         else {
00264           if( debug ) std::cout << "NO substitution" << std::endl;
00265         }
00266       }// end if( PatternMatching )
00267       else  {
00268         if( debug ) std::cout << "NO MATCHING" << std::endl;
00269       }
00270     } // end for values
00271   } // end for rules
00272 
00273   //  static std::map<const StringTreeLeaf*, SString> cache;
00274 
00275   for(size_t i=0; i< container.value.size(); i++)
00276   {
00277     if( substituted[i] == false)
00278     {
00279       const std::pair<StringTreeLeaf, VarNumber> & value_leaf = container.value[i];
00280 
00281       std::string& destination = renamed_value[renamed_index].first;
00282       value_leaf.first.toStr( destination );
00283       renamed_value[renamed_index].second = value_leaf.second ;
00284       renamed_index++;
00285     }
00286   }
00287 }
00288 
00289 SubstitutionRule::SubstitutionRule(const char *pattern, const char *alias, const char *substitution)
00290 {
00291   std::vector<std::string> split_text;
00292   boost::split(split_text, pattern, boost::is_any_of("./"));
00293 
00294   _pattern.reserve(split_text.size());
00295   for (const auto& part: split_text){
00296     if(part.size()>0)  _pattern.push_back( part );
00297   }
00298 
00299   boost::split(split_text, alias, boost::is_any_of("./"));
00300 
00301   _alias.reserve(split_text.size());
00302   for (const auto& part: split_text){
00303     if(part.size()>0)  _alias.push_back( part );
00304   }
00305 
00306   boost::split(split_text, substitution, boost::is_any_of("./"));
00307 
00308   _substitution.reserve(split_text.size());
00309   for (const auto& part: split_text){
00310     if(part.size()>0)  _substitution.push_back( part );
00311   }
00312 }
00313 
00314 
00315 
00316 
00317 } //end namespace


ros_type_introspection
Author(s): Davide Faconti
autogenerated on Sun Oct 1 2017 02:54:53