From 8457b028c3e29f4fe5837d6c6ba50aff66594e86 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mathias=20L=C3=BCdtke?= <mathias.luedtke@ipa.fraunhofer.de>
Date: Tue, 13 Sep 2016 23:37:00 +0200
Subject: [PATCH] automatically unmap PDO if destroyed

---
 canopen_master/include/canopen_master/canopen.h |  6 ++++++
 canopen_master/include/canopen_master/objdict.h |  1 +
 canopen_master/src/objdict.cpp                  | 10 ++++++++++
 canopen_master/src/pdo.cpp                      |  1 +
 4 files changed, 18 insertions(+)

diff --git a/canopen_master/include/canopen_master/canopen.h b/canopen_master/include/canopen_master/canopen.h
index 4055f3953..6bc7e2959 100644
--- a/canopen_master/include/canopen_master/canopen.h
+++ b/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{
diff --git a/canopen_master/include/canopen_master/objdict.h b/canopen_master/include/canopen_master/objdict.h
index 067a035af..39efcd501 100644
--- a/canopen_master/include/canopen_master/objdict.h
+++ b/canopen_master/include/canopen_master/objdict.h
@@ -489,6 +489,7 @@ class ObjectStorage{
     }
 
     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));
diff --git a/canopen_master/src/objdict.cpp b/canopen_master/src/objdict.cpp
index 515c6686f..066b19496 100644
--- a/canopen_master/src/objdict.cpp
+++ b/canopen_master/src/objdict.cpp
@@ -390,6 +390,16 @@ std::pair<ObjectDict::Key, size_t> ObjectStorage::map(uint16_t index, uint8_t su
     }
 }
 
+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_);
diff --git a/canopen_master/src/pdo.cpp b/canopen_master/src/pdo.cpp
index 961c7ea86..afc683e13 100644
--- a/canopen_master/src/pdo.cpp
+++ b/canopen_master/src/pdo.cpp
@@ -131,6 +131,7 @@ void PDOMapper::PDO::parse_and_set_mapping(const ObjectStorageSharedPtr &storage
                 if(read || write) wd = ObjectStorage::WriteDelegate(b.get(), &Buffer::write); // set writer for buffer setup or as write delegate
                 
                 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);
             }