diff --git a/canopen_master/include/canopen_master/canopen.h b/canopen_master/include/canopen_master/canopen.h
index dd589c9b7..b1fe53db3 100644
--- a/canopen_master/include/canopen_master/canopen.h
+++ b/canopen_master/include/canopen_master/canopen.h
@@ -106,6 +106,9 @@ class PDOMapper{
                 tpdo.reset();
             return tpdo;
         }
+        bool operator <(const TPDO& other) const{
+            return frame.id < other.frame.id;
+        }
     private:
         TPDO(const can::CommInterfaceSharedPtr interface) : interface_(interface){}
         bool init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index);
@@ -113,6 +116,15 @@ class PDOMapper{
         boost::mutex mutex;
     };
 
+    template <typename T, typename Compare>
+    struct cmp_ptr
+    {
+        bool operator()(const T lhs, const T rhs) const {
+            Compare c;
+            return c(*lhs, *rhs);
+        }
+    };
+
     struct RPDO : public PDO{
         void sync(LayerStatus &status);
         typedef std::shared_ptr<RPDO> RPDOSharedPtr;
@@ -134,7 +146,7 @@ class PDOMapper{
     };
 
     std::unordered_set<RPDO::RPDOSharedPtr> rpdos_;
-    std::unordered_set<TPDO::TPDOSharedPtr> tpdos_;
+    std::set<TPDO::TPDOSharedPtr,cmp_ptr<TPDO::TPDOSharedPtr, std::less<TPDO>>> tpdos_;
 
     const can::CommInterfaceSharedPtr interface_;
 
diff --git a/canopen_master/src/pdo.cpp b/canopen_master/src/pdo.cpp
index 051ab4ecb..6482a4179 100644
--- a/canopen_master/src/pdo.cpp
+++ b/canopen_master/src/pdo.cpp
@@ -331,7 +331,7 @@ void PDOMapper::read(LayerStatus &status){
 }
 bool PDOMapper::write(){
     boost::mutex::scoped_lock lock(mutex_);
-    for(std::unordered_set<TPDO::TPDOSharedPtr >::iterator it = tpdos_.begin(); it != tpdos_.end(); ++it){
+    for(auto it = tpdos_.begin(); it != tpdos_.end(); ++it){
         (*it)->sync();
     }
     return true; // TODO: check for errors