objdict.h
Go to the documentation of this file.
00001 #ifndef H_OBJDICT
00002 #define H_OBJDICT
00003 
00004 #include <socketcan_interface/FastDelegate.h>
00005 #include <boost/unordered_map.hpp>    
00006 #include <boost/unordered_set.hpp>    
00007 #include <boost/thread/mutex.hpp>    
00008 #include <boost/make_shared.hpp>
00009 #include <boost/function.hpp>
00010 #include <typeinfo> 
00011 #include <vector>
00012 #include "exceptions.h"
00013 
00014 namespace canopen{
00015 
00016 class TypeGuard{
00017     const std::type_info& (*get_type)();
00018     size_t type_size;
00019 
00020     template<typename T> class TypeInfo{
00021     public:
00022         static const std::type_info& id() { return typeid(T); }
00023     };
00024     TypeGuard(const std::type_info& (*ti)(), const size_t s): get_type(ti), type_size(s) {}
00025 public:
00026 
00027     template<typename T> bool is_type() const {
00028         return valid() && get_type() == typeid(T);
00029     }
00030     
00031     bool operator==(const TypeGuard &other) const {
00032         return valid() && other.valid() && (get_type() == other.get_type());
00033     }
00034     
00035     TypeGuard(): get_type(0), type_size(0) {}
00036     bool valid() const { return get_type != 0; }
00037     size_t get_size() const { return type_size; }
00038     template<typename T> static TypeGuard create() { return TypeGuard(TypeInfo<T>::id, sizeof(T)); }
00039 };
00040 
00041 class String: public std::vector<char>{
00042 public:
00043     String() {}
00044     String(const std::string &str) : std::vector<char>(str.begin(), str.end()) {}
00045     operator const char * () const {
00046         return &at(0);
00047     }
00048     operator const std::string () const {
00049         return std::string(begin(), end());
00050     }
00051 };
00052 
00053 class HoldAny{
00054     String buffer;
00055     TypeGuard type_guard;
00056     bool empty;
00057 public:
00058     HoldAny() : empty(true) {}
00059     
00060     const TypeGuard& type() const{ return type_guard; }
00061     
00062     template<typename T> HoldAny(const T &t) : type_guard(TypeGuard::create<T>()), empty(false){
00063         buffer.resize(sizeof(T));
00064         *(T*)&(buffer.front()) = t;
00065     }
00066     HoldAny(const std::string &t): type_guard(TypeGuard::create<std::string>()), empty(false){
00067         if(!type_guard.is_type<std::string>()){
00068             BOOST_THROW_EXCEPTION(std::bad_cast());
00069         }
00070         buffer = t;
00071     }
00072     HoldAny(const TypeGuard &t): type_guard(t), empty(true){ }
00073 
00074     bool is_empty() const { return empty; }
00075     
00076     const String& data() const { 
00077         if(empty){
00078             BOOST_THROW_EXCEPTION(std::length_error("buffer empty"));
00079         }        
00080         return buffer;
00081     }
00082 
00083     template<typename T> const T & get() const{
00084         if(!type_guard.is_type<T>()){
00085             BOOST_THROW_EXCEPTION(std::bad_cast());
00086         }else if(empty){
00087             BOOST_THROW_EXCEPTION(std::length_error("buffer empty"));
00088         }
00089         return *(T*)&(buffer.front());
00090     }
00091 };
00092 
00093 template<> const String & HoldAny::get() const;
00094 
00095 struct DeviceInfo{
00096     std::string vendor_name;
00097     uint32_t vendor_number;
00098     std::string product_name;
00099     uint32_t product_number;
00100     uint32_t revision_number;
00101     std::string order_code;
00102     boost::unordered_set<uint32_t> baudrates;
00103     bool simple_boot_up_master;
00104     bool simple_boot_up_slave;
00105     uint8_t granularity;
00106     bool dynamic_channels_supported;
00107     bool group_messaging;
00108     uint16_t nr_of_rx_pdo;
00109     uint16_t nr_of_tx_pdo;
00110     bool lss_supported;
00111     boost::unordered_set<uint16_t> dummy_usage;
00112 };
00113 
00114 #define THROW_WITH_KEY(e,k) BOOST_THROW_EXCEPTION(boost::enable_error_info(e) << ObjectDict::key_info(k))
00115 
00116 class ObjectDict{
00117 protected:
00118 public:
00119     class Key{
00120         static size_t fromString(const std::string &str);
00121     public:
00122         const size_t hash;
00123         Key(const uint16_t i) : hash((i<<16)| 0xFFFF) {}
00124         Key(const uint16_t i, const uint8_t s): hash((i<<16)| s) {}
00125         Key(const std::string &str): hash(fromString(str)) {}
00126         bool hasSub() const { return (hash & 0xFFFF) != 0xFFFF; }
00127         uint8_t sub_index() const { return hash & 0xFFFF; }
00128         uint16_t index() const { return hash  >> 16;}
00129         bool operator==(const Key &other) const { return hash == other.hash; }
00130         operator std::string() const;
00131     };
00132     enum Code{
00133         NULL_DATA = 0x00,
00134         DOMAIN_DATA = 0x02,
00135         DEFTYPE = 0x05,
00136         DEFSTRUCT = 0x06,
00137         VAR = 0x07,
00138         ARRAY = 0x08,
00139         RECORD = 0x09
00140     };
00141     enum DataTypes{
00142         DEFTYPE_INTEGER8 = 0x0002,
00143         DEFTYPE_INTEGER16 = 0x0003,
00144         DEFTYPE_INTEGER32 = 0x0004,
00145         DEFTYPE_UNSIGNED8 = 0x0005,
00146         DEFTYPE_UNSIGNED16 = 0x0006,
00147         DEFTYPE_UNSIGNED32 = 0x0007,
00148         DEFTYPE_REAL32 = 0x0008,
00149         DEFTYPE_VISIBLE_STRING = 0x0009,
00150         DEFTYPE_OCTET_STRING = 0x000A,
00151         DEFTYPE_UNICODE_STRING = 0x000B,
00152         DEFTYPE_DOMAIN = 0x000F,
00153         DEFTYPE_REAL64 = 0x0010,
00154         DEFTYPE_INTEGER64 = 0x0015,
00155         DEFTYPE_UNSIGNED64 = 0x001B
00156     };
00157     struct Entry{
00158         Code obj_code;
00159         uint16_t index;
00160         uint8_t sub_index;
00161         uint16_t data_type;
00162         bool constant;
00163         bool readable;
00164         bool writable;
00165         bool mappable;
00166         std::string desc;
00167         HoldAny def_val;
00168         HoldAny init_val;
00169         
00170         Entry() {}
00171         
00172         Entry(const Code c, const uint16_t i,  const uint16_t t, const std::string & d, const bool r = true, const bool w = true, bool m = false, const HoldAny def = HoldAny(), const HoldAny init = HoldAny()):
00173         obj_code(c), index(i), sub_index(0),data_type(t),readable(r), writable(w), mappable(m), desc(d), def_val(def), init_val(init) {}
00174         
00175         Entry(const uint16_t i, const uint8_t s, const uint16_t t, const std::string & d, const bool r = true, const bool w = true, bool m = false, const HoldAny def = HoldAny(), const HoldAny init = HoldAny()):
00176         obj_code(VAR), index(i), sub_index(s),data_type(t),readable(r), writable(w), mappable(m), desc(d), def_val(def), init_val(init) {}
00177         
00178         operator Key() const { return Key(index, sub_index); }
00179         const HoldAny & value() const { return !init_val.is_empty() ? init_val : def_val; }
00180             
00181     };
00182     const Entry& operator()(uint16_t i) const{
00183         return *at(Key(i));
00184     }
00185     const Entry& operator()(uint16_t i, uint8_t s) const{
00186         return *at(Key(i,s));
00187     }
00188     const boost::shared_ptr<const Entry>& get(const Key &k) const{
00189         return at(k);
00190     }
00191     bool has(uint16_t i, uint8_t s) const{
00192         return dict_.find(Key(i,s)) != dict_.end();
00193     }
00194     bool has(uint16_t i) const{
00195         return dict_.find(Key(i)) != dict_.end();
00196     }
00197     bool has(const Key &k) const{
00198         return dict_.find(k) != dict_.end();
00199     }
00200     bool insert(bool is_sub, boost::shared_ptr<const Entry> e){
00201         std::pair<boost::unordered_map<Key, boost::shared_ptr<const Entry> >::iterator, bool>  res = dict_.insert(std::make_pair(is_sub?Key(e->index,e->sub_index):Key(e->index),e));
00202         return res.second;
00203     }
00204     bool iterate(boost::unordered_map<Key, boost::shared_ptr<const Entry> >::const_iterator &it) const;
00205     typedef std::list<std::pair<std::string, std::string> > Overlay;
00206     static boost::shared_ptr<ObjectDict> fromFile(const std::string &path, const Overlay &overlay = Overlay());
00207     const DeviceInfo device_info;
00208     
00209     ObjectDict(const DeviceInfo &info): device_info(info) {}
00210     typedef boost::error_info<struct tag_objectdict_key, ObjectDict::Key> key_info;
00211 protected:
00212     const boost::shared_ptr<const Entry>& at(const Key &key) const{
00213         try{
00214             return dict_.at(key);
00215         }
00216         catch(const std::out_of_range &e){
00217             THROW_WITH_KEY(e, key);
00218         }
00219     }
00220 
00221     boost::unordered_map<Key, boost::shared_ptr<const Entry> > dict_;
00222 };
00223 
00224 std::size_t hash_value(ObjectDict::Key const& k);
00225 
00226 template<typename T> class NodeIdOffset{
00227     T offset;
00228     T (*adder)(const uint8_t &, const T &);
00229     
00230     static T add(const uint8_t &u, const T &t) {
00231         return u+t;
00232     }
00233 public:
00234     NodeIdOffset(const T &t): offset(t), adder(add) {}
00235     
00236     static const T apply(const HoldAny &val, const uint8_t &u){
00237         if(!val.is_empty()){
00238             if(TypeGuard::create<T>() ==  val.type() ){
00239                 return val.get<T>();
00240             }else{
00241                 const NodeIdOffset<T> &no = val.get< NodeIdOffset<T> >();                
00242                 return no.adder(u, no.offset);
00243             }
00244         }else{
00245             BOOST_THROW_EXCEPTION(std::bad_cast());
00246         }
00247         
00248     }
00249 };
00250 
00251 template<typename T> std::ostream& operator<<(std::ostream& stream, const NodeIdOffset<T> &n) {
00252     //stream << "Offset: " << n.apply(0);
00253     return stream;
00254  }
00255 std::ostream& operator<<(std::ostream& stream, const ObjectDict::Key &k);
00256 
00257 class AccessException : public Exception{
00258 public:
00259     AccessException(const std::string &w) : Exception(w) {}
00260 };
00261  
00262  
00263 class ObjectStorage{
00264 public:
00265     typedef fastdelegate::FastDelegate2<const ObjectDict::Entry&, String &> ReadDelegate;
00266     typedef fastdelegate::FastDelegate2<const ObjectDict::Entry&, const String &> WriteDelegate;
00267     
00268 protected:
00269     class Data: boost::noncopyable{
00270         boost::mutex mutex;
00271         String buffer;
00272         bool valid;
00273 
00274         ReadDelegate read_delegate;
00275         WriteDelegate write_delegate;
00276         
00277         template <typename T> T & access(){
00278             if(!valid){
00279                 THROW_WITH_KEY(std::length_error("buffer not valid"), key);
00280             }
00281             return *(T*)&(buffer.front());
00282         }
00283         template <typename T> T & allocate(){
00284             if(!valid){
00285                 buffer.resize(sizeof(T));
00286                 valid = true;
00287             }
00288             return access<T>();
00289         }
00290     public:
00291         const TypeGuard type_guard;
00292         const boost::shared_ptr<const ObjectDict::Entry> entry;
00293         const ObjectDict::Key key;
00294         size_t size() { boost::mutex::scoped_lock lock(mutex); return buffer.size(); }
00295         
00296         template<typename T> Data(const ObjectDict::Key &k, const boost::shared_ptr<const ObjectDict::Entry> &e, const T &val, const ReadDelegate &r, const WriteDelegate &w)
00297         : valid(false), read_delegate(r), write_delegate(w), type_guard(TypeGuard::create<T>()), entry(e), key(k){
00298             assert(!r.empty());
00299             assert(!w.empty());
00300             assert(e);
00301             allocate<T>() = val;
00302         }
00303         Data(const ObjectDict::Key &k, const boost::shared_ptr<const ObjectDict::Entry> &e, const TypeGuard &t, const ReadDelegate &r, const WriteDelegate &w)
00304         : valid(false), read_delegate(r), write_delegate(w), type_guard(t), entry(e), key(k){
00305             assert(!r.empty());
00306             assert(!w.empty());
00307             assert(e);
00308             assert(t.valid());
00309             buffer.resize(t.get_size());
00310         }
00311         void set_delegates(const ReadDelegate &r, const WriteDelegate &w){
00312             boost::mutex::scoped_lock lock(mutex);
00313             if(r) read_delegate = r;
00314             if(w) write_delegate = w;
00315         }
00316         template<typename T> const T get(bool cached) {
00317             boost::mutex::scoped_lock lock(mutex);
00318             
00319             if(!entry->readable){
00320                 THROW_WITH_KEY(AccessException("no read access"), key);
00321 
00322             }
00323             
00324             if(entry->constant) cached = true;
00325             
00326             if(!valid || !cached){
00327                 allocate<T>();
00328                 read_delegate(*entry, buffer);
00329             }
00330             return access<T>();
00331         }
00332         template<typename T>  void set(const T &val) {
00333             boost::mutex::scoped_lock lock(mutex);
00334             
00335             if(!entry->writable){
00336                 if(access<T>() != val){
00337                     THROW_WITH_KEY(AccessException("no write access"), key);
00338                 }
00339             }else{
00340                 allocate<T>() = val;
00341                 write_delegate(*entry, buffer);
00342             }
00343         }
00344         template<typename T>  void set_cached(const T &val) {
00345             boost::mutex::scoped_lock lock(mutex);
00346             if(!valid || val != access<T>() ){
00347                 if(!entry->writable){
00348                     THROW_WITH_KEY(AccessException("no write access and not cached"), key);
00349                 }else{
00350                     allocate<T>() = val;
00351                     write_delegate(*entry, buffer);
00352                 }
00353             }
00354         }
00355         void init();
00356         void reset();
00357         void force_write();
00358 
00359     };        
00360         
00361 public:
00362     template<const uint16_t dt> struct DataType{
00363         typedef void type;
00364     };
00365     
00366     template<typename T> class Entry{
00367         boost::shared_ptr<Data> data;
00368     public:
00369         typedef T type;
00370         bool valid() const { return data != 0; }
00371         const T get() {
00372             if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::get()") );
00373 
00374             return data->get<T>(false);
00375         }    
00376         bool get(T & val){
00377             try{
00378                 val = get();
00379                 return true;
00380             }catch(...){
00381                 return false;
00382             }
00383         }    
00384         const T get_cached() {
00385             if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::get_cached()") );
00386 
00387             return data->get<T>(true);
00388         }        
00389         bool get_cached(T & val){
00390             try{
00391                 val = get_cached();
00392                 return true;
00393             }catch(...){
00394                 return false;
00395             }
00396         }    
00397         void set(const T &val) {
00398             if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::set(val)") );
00399             data->set(val);
00400         }
00401         bool set_cached(const T &val) {
00402             if(!data) return false;
00403             try{
00404                     data->set_cached(val);
00405                                 return true;
00406             }catch(...){
00407                 return false;
00408             }
00409         }
00410  
00411         Entry() {}
00412         Entry(boost::shared_ptr<Data> &d)
00413         : data(d){
00414             assert(data);
00415         }
00416         Entry(boost::shared_ptr<ObjectStorage> storage, uint16_t index)
00417         : data(storage->entry<type>(index).data) {
00418             assert(data);
00419         }
00420         Entry(boost::shared_ptr<ObjectStorage> storage, uint16_t index, uint8_t sub_index)
00421         : data(storage->entry<type>(index, sub_index).data) {
00422             assert(data);
00423         }
00424         Entry(boost::shared_ptr<ObjectStorage> storage, const ObjectDict::Key &k)
00425         : data(storage->entry<type>(k).data) {
00426             assert(data);
00427         }
00428         const ObjectDict::Entry & desc() const{
00429             return *(data->entry);
00430         }
00431     };
00432     
00433     void reset();
00434     
00435 protected:
00436     boost::unordered_map<ObjectDict::Key, boost::shared_ptr<Data> > storage_;
00437     boost::mutex mutex_;
00438     
00439     void init_nolock(const ObjectDict::Key &key, const boost::shared_ptr<const ObjectDict::Entry> &entry);
00440     
00441     ReadDelegate read_delegate_;
00442     WriteDelegate write_delegate_;
00443     size_t map(const boost::shared_ptr<const ObjectDict::Entry> &e, const ObjectDict::Key &key, const ReadDelegate & read_delegate, const WriteDelegate & write_delegate);
00444 public:
00445     template<typename T> Entry<T> entry(const ObjectDict::Key &key){
00446         boost::mutex::scoped_lock lock(mutex_);
00447         
00448         boost::unordered_map<ObjectDict::Key, boost::shared_ptr<Data> >::iterator it = storage_.find(key);
00449         
00450         if(it == storage_.end()){
00451             const boost::shared_ptr<const ObjectDict::Entry> e = dict_->get(key);
00452             
00453             boost::shared_ptr<Data> data;
00454             TypeGuard type = TypeGuard::create<T>();
00455     
00456             if(!e->def_val.is_empty()){
00457                 T val = NodeIdOffset<T>::apply(e->def_val, node_id_);
00458                 data = boost::make_shared<Data>(key, e,val, read_delegate_, write_delegate_);
00459             }else{
00460                 if(!e->def_val.type().valid() ||  e->def_val.type() == type) {
00461                     data = boost::make_shared<Data>(key,e,type, read_delegate_, write_delegate_);
00462                 }else{
00463                     THROW_WITH_KEY(std::bad_cast(), key);
00464                 }
00465             }
00466             
00467             std::pair<boost::unordered_map<ObjectDict::Key, boost::shared_ptr<Data> >::iterator, bool>  ok = storage_.insert(std::make_pair(key, data));
00468             it = ok.first;
00469         }
00470         
00471         if(!it->second->type_guard.is_type<T>()){
00472             THROW_WITH_KEY(std::bad_cast(), key);
00473         }
00474         return Entry<T>(it->second);
00475     }
00476 
00477     size_t map(uint16_t index, uint8_t sub_index, const ReadDelegate & read_delegate, const WriteDelegate & write_delegate);
00478     
00479     template<typename T> Entry<T> entry(uint16_t index){
00480         return entry<T>(ObjectDict::Key(index));
00481     }
00482     template<typename T> Entry<T> entry(uint16_t index, uint8_t sub_index){
00483         return entry<T>(ObjectDict::Key(index,sub_index));
00484     }
00485     
00486     template<typename T> void entry(Entry<T> &e, uint16_t index){ // TODO: migrate to bool
00487         e = entry<T>(ObjectDict::Key(index));
00488     }
00489     template<typename T> void entry(Entry<T> &e, uint16_t index, uint8_t sub_index){  // TODO: migrate to bool
00490         e = entry<T>(ObjectDict::Key(index,sub_index));
00491     }
00492     template<typename T> bool entry(Entry<T> &e, const ObjectDict::Key &k){
00493         try{
00494             e = entry<T>(k);
00495             return true;
00496         }catch(...){
00497             return false;
00498         }
00499     }
00500      boost::function<std::string()> getStringReader(const ObjectDict::Key &key, bool cached = false);
00501      boost::function<void(const std::string &)> getStringWriter(const ObjectDict::Key &key, bool cached = false);
00502 
00503     const boost::shared_ptr<const ObjectDict> dict_;
00504     const uint8_t node_id_;
00505     
00506     ObjectStorage(boost::shared_ptr<const ObjectDict> dict, uint8_t node_id, ReadDelegate read_delegate, WriteDelegate write_delegate);
00507     
00508     void init(const ObjectDict::Key &key);
00509     void init_all();
00510 };
00511 
00512 template<> String & ObjectStorage::Data::access();
00513 template<> String & ObjectStorage::Data::allocate();
00514 
00515 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER8> { typedef int8_t type;};
00516 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER16> { typedef int16_t type;};
00517 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER32> { typedef int32_t type;};
00518 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER64> { typedef int64_t type;};
00519 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED8> { typedef uint8_t type;};
00520 
00521 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED16> { typedef uint16_t type;};
00522 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED32> { typedef uint32_t type;};
00523 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED64> { typedef uint64_t type;};
00524 
00525 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_REAL32> { typedef float type;};
00526 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_REAL64> { typedef double type;};
00527 
00528 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_VISIBLE_STRING> { typedef String type;};
00529 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_OCTET_STRING> { typedef String type;};
00530 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNICODE_STRING> { typedef String type;};
00531 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_DOMAIN> { typedef String type;};
00532 
00533 template<typename T, typename R> static R *branch_type(const uint16_t data_type){
00534     switch(ObjectDict::DataTypes(data_type)){
00535         case ObjectDict::DEFTYPE_INTEGER8: return T::template func< ObjectDict::DEFTYPE_INTEGER8 >;
00536         case ObjectDict::DEFTYPE_INTEGER16: return T::template func< ObjectDict::DEFTYPE_INTEGER16 >;
00537         case ObjectDict::DEFTYPE_INTEGER32: return T::template func< ObjectDict::DEFTYPE_INTEGER32 >;
00538         case ObjectDict::DEFTYPE_INTEGER64: return T::template func< ObjectDict::DEFTYPE_INTEGER64 >;
00539             
00540         case ObjectDict::DEFTYPE_UNSIGNED8: return T::template func< ObjectDict::DEFTYPE_UNSIGNED8 >;
00541         case ObjectDict::DEFTYPE_UNSIGNED16: return T::template func< ObjectDict::DEFTYPE_UNSIGNED16 >;
00542         case ObjectDict::DEFTYPE_UNSIGNED32: return T::template func< ObjectDict::DEFTYPE_UNSIGNED32 >;
00543         case ObjectDict::DEFTYPE_UNSIGNED64: return T::template func< ObjectDict::DEFTYPE_UNSIGNED64 >;
00544             
00545         case ObjectDict::DEFTYPE_REAL32: return T::template func< ObjectDict::DEFTYPE_REAL32 >;
00546         case ObjectDict::DEFTYPE_REAL64: return T::template func< ObjectDict::DEFTYPE_REAL64 >;
00547 
00548         case ObjectDict::DEFTYPE_VISIBLE_STRING: return T::template func< ObjectDict::DEFTYPE_VISIBLE_STRING >;
00549         case ObjectDict::DEFTYPE_OCTET_STRING: return T::template func< ObjectDict::DEFTYPE_OCTET_STRING >;
00550         case ObjectDict::DEFTYPE_UNICODE_STRING: return T::template func< ObjectDict::DEFTYPE_UNICODE_STRING >;
00551         case ObjectDict::DEFTYPE_DOMAIN: return T::template func< ObjectDict::DEFTYPE_DOMAIN >;
00552            
00553         default: return 0;
00554     }
00555 }
00556 
00557 } // canopen
00558 
00559 #endif // !H_OBJDICT


canopen_master
Author(s): Mathias Lüdtke
autogenerated on Sun Sep 3 2017 03:10:42