automatically unmap PDO if destroyed

ros-industrial · Sep 13, 2016 · 84e9ffc · 84e9ffc
1 parent 89b52df
commit 84e9ffc
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 0 deletions.
diff --git 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{
 
@@ -86,9 +87,14 @@ class PDOMapper{
     class PDO {
     protected:
         void parse_and_set_mapping(const boost::shared_ptr<ObjectStorage> &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< boost::shared_ptr<Buffer> >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
@@ -478,6 +478,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
@@ -381,6 +381,15 @@ 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(boost::shared_ptr<const ObjectDict> 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
@@ -131,6 +131,7 @@ void PDOMapper::PDO::parse_and_set_mapping(const boost::shared_ptr<ObjectStorage
                 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);
             }