check for doubled PDO mappings

canopen_master/include/canopen_master/canopen.h

@@ -12,6 +12,7 @@
 #include <boost/thread/condition_variable.hpp>
 #include <boost/chrono/system_clocks.hpp>
 #include <boost/lexical_cast.hpp>
+#include <boost/function.hpp>
 
 namespace canopen{
 
@@ -87,9 +88,14 @@ class PDOMapper{
     class PDO {
     protected:
         void parse_and_set_mapping(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write);
+        boost::function<void()> unmap;
         can::Frame frame;
         uint8_t transmission_type;
         std::vector<BufferSharedPtr>buffers;
+    public:
+        ~PDO(){
+            if(unmap) unmap();
+        }
     };
 
     struct TPDO: public PDO{

canopen_master/include/canopen_master/objdict.h

@@ -323,6 +323,10 @@ class ObjectStorage{
             if(r) read_delegate = r;
             if(w) write_delegate = w;
         }
+        bool test_delegates(const ReadDelegate &r, const WriteDelegate &w){
+            boost::mutex::scoped_lock lock(mutex);
+            return (!r || read_delegate == r) && (!w || write_delegate == w);
+        }
         template<typename T> const T get(bool cached) {
             boost::mutex::scoped_lock lock(mutex);
 
@@ -450,7 +454,7 @@ class ObjectStorage{
 
     ReadDelegate read_delegate_;
     WriteDelegate write_delegate_;
-    size_t map(const ObjectDict::EntryConstSharedPtr &e, const ObjectDict::Key &key, const ReadDelegate & read_delegate, const WriteDelegate & write_delegate);
+    std::pair<ObjectDict::Key, size_t> map(const ObjectDict::EntryConstSharedPtr &e, const ObjectDict::Key &key, const ReadDelegate & read_delegate, const WriteDelegate & write_delegate);
 public:
     template<typename T> Entry<T> entry(const ObjectDict::Key &key){
         boost::mutex::scoped_lock lock(mutex_);
@@ -484,7 +488,8 @@ class ObjectStorage{
         return Entry<T>(it->second);
     }
 
-    size_t map(uint16_t index, uint8_t sub_index, const ReadDelegate & read_delegate, const WriteDelegate & write_delegate);
+    std::pair<ObjectDict::Key, size_t> map(uint16_t index, uint8_t sub_index, const ReadDelegate & read_delegate, const WriteDelegate & write_delegate);
+    void unmap(const ObjectDict::Key &key);
 
     template<typename T> Entry<T> entry(uint16_t index){
         return entry<T>(ObjectDict::Key(index));

canopen_master/src/objdict.cpp

@@ -331,7 +331,7 @@ ObjectDictSharedPtr ObjectDict::fromFile(const std::string &path, const ObjectDi
     return dict;
 }
 
-size_t ObjectStorage::map(const ObjectDict::EntryConstSharedPtr &e, const ObjectDict::Key &key, const ReadDelegate & read_delegate, const WriteDelegate & write_delegate){
+std::pair<ObjectDict::Key, size_t> ObjectStorage::map(const ObjectDict::EntryConstSharedPtr &e, const ObjectDict::Key &key, const ReadDelegate & read_delegate, const WriteDelegate & write_delegate){
     boost::unordered_map<ObjectDict::Key, DataSharedPtr >::iterator it = storage_.find(key);
 
     if(it == storage_.end()){
@@ -352,19 +352,28 @@ size_t ObjectStorage::map(const ObjectDict::EntryConstSharedPtr &e, const Object
     }
 
     if(read_delegate && write_delegate){
+        if(!it->second->test_delegates(read_delegate_, write_delegate_)){
+            THROW_WITH_KEY(std::logic_error("PDO was mapped twice, this is not yet supported (#183)") , key);
+        }
         it->second->set_delegates(read_delegate_, write_delegate);
         it->second->force_write(); // update buffer
         it->second->set_delegates(read_delegate, write_delegate_);
     }else if(write_delegate) {
+        if(!it->second->test_delegates(0, write_delegate_)){
+            THROW_WITH_KEY(std::logic_error("PDO was mapped twice, this is not yet supported (#183)") , key);
+        }
         it->second->set_delegates(read_delegate_, write_delegate);
         it->second->force_write(); // update buffer
     }else if(read_delegate){
+        if(!it->second->test_delegates(read_delegate_, 0)){
+            THROW_WITH_KEY(std::logic_error("PDO was mapped twice, this is not yet supported (#183)") , key);
+        }
         it->second->set_delegates(read_delegate, write_delegate_);
     }
-    return it->second->size();
+    return std::make_pair(key, it->second->size());
 }
 
-size_t ObjectStorage::map(uint16_t index, uint8_t sub_index, const ReadDelegate & read_delegate, const WriteDelegate & write_delegate){
+std::pair<ObjectDict::Key, size_t> ObjectStorage::map(uint16_t index, uint8_t sub_index, const ReadDelegate & read_delegate, const WriteDelegate & write_delegate){
     boost::mutex::scoped_lock lock(mutex_);
 
     try{
@@ -381,6 +390,16 @@ size_t ObjectStorage::map(uint16_t index, uint8_t sub_index, const ReadDelegate
     }
 }
 
+void ObjectStorage::unmap(const ObjectDict::Key &key){
+    boost::mutex::scoped_lock lock(mutex_);
+    boost::unordered_map<ObjectDict::Key, boost::shared_ptr<Data> >::iterator it = storage_.find(key);
+
+    if(it != storage_.end()){
+        it->second->set_delegates(read_delegate_, write_delegate_);
+    }
+}
+
+
 ObjectStorage::ObjectStorage(ObjectDictConstSharedPtr dict, uint8_t node_id, ReadDelegate read_delegate, WriteDelegate write_delegate)
 :read_delegate_(read_delegate), write_delegate_(write_delegate), dict_(dict), node_id_(node_id){
     assert(dict_);

canopen_master/src/pdo.cpp

@@ -129,8 +129,10 @@ void PDOMapper::PDO::parse_and_set_mapping(const ObjectStorageSharedPtr &storage
                 ObjectStorage::WriteDelegate wd;
                 if(read) rd = ObjectStorage::ReadDelegate(b.get(), &Buffer::read);
                 if(read || write) wd = ObjectStorage::WriteDelegate(b.get(), &Buffer::write); // set writer for buffer setup or as write delegate
-                size_t l = storage->map(param.index, param.sub_index, rd, wd);
-                assert(l  == param.length/8);
+
+                std::pair<ObjectDict::Key, size_t> m = storage->map(param.index, param.sub_index, rd, wd);
+                unmap = boost::bind(&ObjectStorage::unmap, storage, m.first);
+                assert(m.second == param.length/8);
             }
 
             frame.dlc += b->size;