diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..378eac2
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+build
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f4dc38a..2c3a5f9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -26,8 +26,8 @@ set(SOURCES
     src/ConfigData.cpp
 )
 set(HEADERS
-    src/ConfigData.h
-    src/FIFOMap.h
+    src/ConfigData.hpp
+    src/FIFOMap.hpp
 )
 
 add_library(${PROJECT_NAME} SHARED ${SOURCES})
@@ -58,6 +58,11 @@ install(TARGETS ${PROJECT_NAME} ${_INSTALL_DESTINATIONS})
 # Install headers into mars include directory
 install(FILES ${HEADERS} DESTINATION include/configmaps)
 
+# MZ: some compatiblity headers after renaming. keep around for a month, then
+# delete.
+install(FILES src/ConfigData.h DESTINATION include/configmaps)
+install(FILES src/FIFOMap.h DESTINATION include/configmaps)
+
 # Prepare and install necessary files to support finding of the library 
 # using pkg-config
 configure_file(${PROJECT_NAME}.pc.in ${CMAKE_BINARY_DIR}/${PROJECT_NAME}.pc @ONLY)
diff --git a/doc/configmaps.md b/doc/configmaps.md
index 63b1ba4..d7a8421 100644
--- a/doc/configmaps.md
+++ b/doc/configmaps.md
@@ -12,7 +12,7 @@ The ConfigMap is a nested implementation of std::maps and std::vectors to provid
  - double
  - bool
 
-An important feature is the loading and storing of ConfigMaps in yaml files. The example shows the usage of ConfigMaps for further details check [ConfigMap](@ref mars::utils::ConfigData.h).
+An important feature is the loading and storing of ConfigMaps in yaml files. The example shows the usage of ConfigMaps for further details check [ConfigMap](@ref mars::utils::ConfigData.hpp).
 
 Note: While reading and writing yaml files the order of dictionaries is not necessarily kept.
 
@@ -28,7 +28,7 @@ Yaml example file:
 
 Example program:
 
-    #include <configmaps/ConfigData.h>
+    #include <configmaps/ConfigData.hpp>
 
     int main(int argc, char *argv[]) {
 
diff --git a/src/ConfigData.cpp b/src/ConfigData.cpp
index 245aa52..e9c5d18 100644
--- a/src/ConfigData.cpp
+++ b/src/ConfigData.cpp
@@ -1,4 +1,4 @@
-#include "ConfigData.h"
+#include "ConfigData.hpp"
 
 #include <yaml-cpp/yaml.h>
 #include <fstream>
diff --git a/src/ConfigData.h b/src/ConfigData.h
index 63e99d1..b4f7d12 100644
--- a/src/ConfigData.h
+++ b/src/ConfigData.h
@@ -1,732 +1,5 @@
-/*
- *  Copyright 2011, 2012, DFKI GmbH Robotics Innovation Center
- *
- *  This file is part of the MARS simulation framework.
- *
- *  MARS is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU Lesser General Public License
- *  as published by the Free Software Foundation, either version 3
- *  of the License, or (at your option) any later version.
- *
- *  MARS is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU Lesser General Public License for more details.
- *
- *   You should have received a copy of the GNU Lesser General Public License
- *   along with MARS.  If not, see <http://www.gnu.org/licenses/>.
- *
- */
-
-#ifndef CONFIG_DATA_H
-#define CONFIG_DATA_H
-
-#ifdef _PRINT_HEADER_
-  #warning "ConfigData.h"
-#endif
-
-#include <string>
-#include <stdexcept>
-#include <vector>
-#include <cstdio>
-#include <iosfwd>
-
-#include "FIFOMap.h"
-
-namespace configmaps {
-
-    // only functions used from misc.h
-    std::string getPathOfFile(const std::string &filename);
-    std::string trim(const std::string& str);
-
-    class ConfigMap;
-    class ConfigItem;
-
-    template <typename T>
-    class ConfigVectorTemplate : public std::vector<T>{
-    public:
-
-      ConfigVectorTemplate(std::string s) : parentName(s) {}
-      ConfigVectorTemplate() : parentName("") {}
-
-      T& operator[](short index) {
-        return (*this)[(unsigned long)index];
-      }
-
-      T& operator[](unsigned short index) {
-        return (*this)[(unsigned long)index];
-      }
-
-      T& operator[](unsigned long index) {
-        if(index == this->size()) {
-          this->push_back(T());
-          this->back().setParentName(parentName);
-        }
-        return std::vector<T>::operator[](index);
-      }
-
-      T& operator[](long index) {
-        return (*this)[(unsigned long)index];
-      }
-
-      T& operator[](int index) {
-        return (*this)[(unsigned long)index];
-      }
-
-      T& operator[](unsigned int index) {
-        return (*this)[(unsigned long)index];
-      }
-
-      const T& operator[](unsigned long index) const {
-        return std::vector<T>::operator[](index);
-      }
-
-       ConfigVectorTemplate& operator[](const char *text) {
-        if(0 == this->size()) {
-          this->push_back(T());
-          this->back().setParentName(parentName);
-        }
-        return std::vector<T>::operator[](0)[text];
-      }
-
-      const ConfigVectorTemplate& operator[](const char *text) const {
-        return std::vector<T>::operator[](0)[text];
-      }
-
-      bool isVector() const {
-        return (this->size() > 1);
-      }
-
-      bool isMap() const {
-        if(this->size() == 1 && std::vector<T>::operator[](0).children.size() > 0) return true;
-        return false;
-      }
-
-      bool isAtom() const {
-        return (this->size() == 1 && std::vector<T>::operator[](0).children.size() == 0);
-      }
-
-      const char* c_str() {
-        return std::vector<T>::operator[](0).getString().c_str();
-      }
-
-      template <typename T1>
-      ConfigVectorTemplate& operator<<(const T1 &v) {
-        return (std::vector<T>::operator[](0) = v);
-      }
-
-      size_t append(const T &item) {
-        this->push_back(item);
-        this->back().setParentName(parentName);
-        return this->size() - 1;
-      }
-
-      ConfigVectorTemplate& operator+=(const T &item) {
-        this->push_back(item);
-        this->back().setParentName(parentName);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator=(const T &item) {
-        this->clear();
-        this->push_back(item);
-        this->back().setParentName(parentName);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator=(int v) {
-        *this = T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator=(bool v) {
-        *this = T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator=(unsigned int v) {
-        *this = T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator=(double v) {
-        *this = T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator=(unsigned long v) {
-        *this = T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator=(std::string v) {
-        *this = T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator=(const char* v) {
-        *this = T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator=(const ConfigMap &v) {
-        *this = T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator+=(const int& v) {
-        *this += T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator+=(const bool& v) {
-        *this += T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator+=(const unsigned int& v) {
-        *this += T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator+=(const double &v) {
-        *this += T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator+=(const unsigned long& v) {
-        *this += T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator+=(const std::string& v) {
-        *this += T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator+=(const char* v) {
-        *this += T(v);
-        return *this;
-      }
-
-      ConfigVectorTemplate& operator+=(const ConfigMap &v) {
-        *this += T(v);
-        return *this;
-      }
-
-      FIFOMap<std::string, ConfigVectorTemplate<ConfigItem> >::iterator beginMap() {
-        return (*this)[0].children.begin();
-      }
-
-      FIFOMap<std::string, ConfigVectorTemplate<ConfigItem> >::iterator endMap() {
-        return (*this)[0].children.end();
-      }
-
-      FIFOMap<std::string, ConfigVectorTemplate<ConfigItem> >::iterator find(const std::string &key) {
-        return (*this)[0].children.find(key);
-      }
-
-      bool hasKey(const std::string &key) {
-        return (*this)[0].children.hasKey(key);
-      }
-
-      std::string toString() {
-        return (*this)[0].toString();
-      }
-
-      operator ConfigMap & () {
-        return (*this)[0].children;
-      }
-
-      operator ConfigMap * () {
-        return &(*this)[0].children;
-      }
-
-      operator ConfigItem & () {
-        return (*this)[0];
-      }
-
-      operator ConfigItem * () {
-        return &(*this)[0];
-      }
-
-      operator int () {
-        return (*this)[0].getInt();
-      }
-
-      operator double () {
-        return (*this)[0].getDouble();
-      }
-
-      operator unsigned int () {
-        return (*this)[0].getUInt();
-      }
-
-      operator unsigned long () {
-        return (*this)[0].getULong();
-      }
-
-      operator std::string () {
-        return (std::string)(*this)[0].getString().c_str();
-      }
-
-      std::string getString() {
-        return (std::string)(*this)[0].getString().c_str();
-      }
-
-      operator bool () {
-        return (*this)[0].getBool();
-      }
-
-      inline void setParentName(std::string s) {
-        parentName = s;
-      }
-
-      inline const std::string& getParentName() const {
-        return parentName;
-      }
-
-    private:
-      std::string parentName;
-
-    }; // end of class ConfigVectorTemplate
-
-    class ConfigItem;
-
-    // these typedefs are here for two reasons.
-    // 1) they make the types nicer to use
-    // 2) this enables us to use a ConfigMap inside the ConfigItem!
-    typedef ConfigVectorTemplate<ConfigItem> ConfigVector;
-
-    template <typename T>
-    T& operator<<(T &a, ConfigVector& v) {
-      return (a = (T)(v[0]));
-    }
-
-    class ConfigMap : public FIFOMap<std::string, ConfigVector> {
-    public:
-      ConfigVector& operator[](const std::string &name) {
-        if(find(name) == end()) {
-          return FIFOMap<std::string, ConfigVector>::operator[](name) = ConfigVector(name);
-        }
-        return FIFOMap<std::string, ConfigVector>::operator[](name);
-      }
-
-      void toYamlStream(std::ostream &out) const;
-      void toYamlFile(const std::string &filename) const;
-      std::string toYamlString() const;
-      static ConfigMap fromYamlStream(std::istream &in);
-      static ConfigMap fromYamlFile(const std::string &filename,
-                                    bool loadURI = false);
-      static ConfigMap fromYamlString(const std::string &s);
-      static void recursiveLoad(ConfigMap *map, std::string path);
-
-      bool hasKey(const std::string &key) const {
-        return (find(key) != end());
-      }
-
-      // checks if the key is in the list, if not return the given default value
-      template<typename T> T get(const std::string &key,
-                                 const T &defaultValue) {
-        if(find(key) != end()) {
-          return (T)(*this)[key];
-        }
-        return defaultValue;
-      }
-
-      // checks if the key is in the list, if not add the given default value
-      // and return it
-      template<typename T> T getOrCreate(const std::string &key,
-                                         const T &defaultValue) {
-        if(find(key) != end()) {
-          return (T)(*this)[key];
-        }
-        (*this)[key] = defaultValue;
-        return defaultValue;
-      }
-
-    };
-
-    // todo: support vector and quaternion
-    // a vector has three children (x, y, z) which can be parsed
-
-    class ConfigItem {
-    public:
-      ConfigMap children;
-      enum ItemType {UNDEFINED_TYPE, INT_TYPE, UINT_TYPE, DOUBLE_TYPE,
-                     ULONG_TYPE, STRING_TYPE, BOOL_TYPE};
-
-      ConfigItem() : luValue(0), iValue(0), uValue(0), dValue(0.0),
-                     parsed(false), type(UNDEFINED_TYPE) {}
-
-      ConfigItem(int val) : luValue(0), iValue(val),
-                                     uValue(0), dValue(0.0),
-                                     parsed(true), type(INT_TYPE) {}
-
-       ConfigItem(bool val) : luValue(0), iValue(val),
-                                      uValue(0), dValue(0.0),
-                                      parsed(true), type(BOOL_TYPE) {}
-
-       ConfigItem(unsigned int val) : luValue(0), iValue(0),
-                                              uValue(val), dValue(0.0),
-                                              parsed(true), type(UINT_TYPE) {}
-
-       ConfigItem(double val) : luValue(0), iValue(0),
-                                        uValue(0), dValue(val),
-                                        parsed(true), type(DOUBLE_TYPE) {}
-
-       ConfigItem(unsigned long val) : luValue(val), iValue(0),
-                                               uValue(0), dValue(0.0),
-                                               parsed(true), type(ULONG_TYPE) {}
-
-       ConfigItem(std::string val) : luValue(0), iValue(0),
-                                             uValue(0), dValue(0.0),
-                                             sValue(val.c_str()), parsed(false),
-                                             type(UNDEFINED_TYPE) {}
-
-       ConfigItem(const char *val) : luValue(0), iValue(0),
-                                             uValue(0), dValue(0.0),
-                                             sValue(val), parsed(false),
-                                             type(UNDEFINED_TYPE) {}
-
-       ConfigItem(ConfigMap map) : children(map), luValue(0), iValue(0),
-                                           uValue(0), dValue(0.0),
-                                           sValue(""), parsed(false),
-                                           type(UNDEFINED_TYPE) {}
-
-      ConfigVector& operator[](const std::string &name) {
-        return children[name];
-      }
-
-      ConfigVector& operator[](const char* name) {
-        return children[name];
-      }
-
-      operator ConfigMap& () {
-        return children;
-      }
-
-      operator ConfigMap* () {
-        return &children;
-      }
-
-      operator ConfigVector& () {
-        throw 1;
-      }
-
-      operator ConfigVector* () {
-        throw 1;
-      }
-
-      operator int () {
-        return getInt();
-      }
-
-      operator double () {
-        return getDouble();
-      }
-
-      operator unsigned int () {
-        return getUInt();
-      }
-
-      operator unsigned long () {
-        return getULong();
-      }
-
-      operator std::string () {
-        return (std::string)getString().c_str();
-      }
-
-      operator bool () {
-        return getBool();
-      }
-
-      inline ItemType getType() const {
-        return type;
-      }
-
-
-      inline void setParentName(std::string s) {
-        parentName = s;
-      }
-
-      inline const std::string& getParentName() const {
-        return parentName;
-      }
-
-      inline bool testType(ItemType _type) {
-        if(type == UNDEFINED_TYPE) {
-          switch(_type) {
-          case INT_TYPE:
-            return parseInt();
-          case DOUBLE_TYPE:
-            return parseDouble();
-          case UINT_TYPE:
-            return parseUInt();
-          case ULONG_TYPE:
-            return parseULong();
-          case STRING_TYPE:
-            return parseString();
-          case BOOL_TYPE:
-            return parseBool();
-          default:
-            return false;
-          }
-        }
-        else if(type == _type) return true;
-        return false;
-      }
-
-      inline int getInt() {
-        if(type != UNDEFINED_TYPE && type != INT_TYPE) {
-          char text[50];
-          sprintf(text, "ConfigItem parsing wrong type getInt: %s - %s",
-                  parentName.c_str(), sValue.c_str());
-          throw std::runtime_error(text);
-        }
-
-        if(!parsed) parseInt();
-        if(parsed) type = INT_TYPE;
-
-        return iValue;
-      }
-
-      inline double getDouble() {
-        if(type != UNDEFINED_TYPE && type != DOUBLE_TYPE) {
-          char text[50];
-          sprintf(text, "ConfigItem parsing wrong type getDouble: %s - %s",
-                  parentName.c_str(), sValue.c_str());
-          throw std::runtime_error(text);
-        }
-        if(!parsed) parseDouble();
-        if(parsed) type = DOUBLE_TYPE;
-        return dValue;
-      }
-
-      inline unsigned int getUInt() {
-        if(type != UNDEFINED_TYPE && type != UINT_TYPE) {
-          char text[50];
-          sprintf(text, "ConfigItem parsing wrong type getInt: %s - %s",
-                  parentName.c_str(), sValue.c_str());
-          throw std::runtime_error(text);
-        }
-
-        if(!parsed) parseUInt();
-        if(parsed) type = UINT_TYPE;
-
-        return uValue;
-      }
-
-      inline unsigned long getULong() {
-        if(type != UNDEFINED_TYPE && type != ULONG_TYPE) {
-          char text[50];
-          sprintf(text, "ConfigItem parsing wrong type getULong: %s - %s",
-                  parentName.c_str(), sValue.c_str());
-          throw std::runtime_error(text);
-        }
-        if(!parsed) parseULong();
-        if(parsed) type = ULONG_TYPE;
-        return luValue;
-      }
-
-      inline std::string getString() {
-        if(type != UNDEFINED_TYPE && type != STRING_TYPE) {
-          char text[50];
-          sprintf(text, "ConfigItem parsing wrong type getString: %s - %s",
-                  parentName.c_str(), sValue.c_str());
-          throw std::runtime_error(text);
-        }
-        if(!parsed) parseString();
-        if(parsed) type = STRING_TYPE;
-        return sValue.c_str();
-      }
-
-      inline std::string getUnparsedString() const {
-        return sValue.c_str();
-      }
-
-      inline int getBool() {
-        if(type != UNDEFINED_TYPE && type != BOOL_TYPE) {
-          char text[50];
-          sprintf(text, "ConfigItem parsing wrong type getBool: %s - %s",
-                  parentName.c_str(), sValue.c_str());
-          throw std::runtime_error(text);
-        }
-
-        if(!parsed) parseBool();
-        if(parsed) type = BOOL_TYPE;
-
-        return iValue;
-      }
-
-      inline void setInt(int value) {
-        iValue = value;
-        parsed = true;
-        type = INT_TYPE;
-      }
-
-      inline void setDouble(double value) {
-        dValue = value;
-        parsed = true;
-        type = DOUBLE_TYPE;
-      }
-
-      inline void setUInt(unsigned int value) {
-        uValue = value;
-        parsed = true;
-        type = UINT_TYPE;
-      }
-
-      inline void setULong(unsigned long value) {
-        luValue = value;
-        parsed = true;
-        type = ULONG_TYPE;
-      }
-
-      inline void setString(const std::string &value) {
-        sValue = value.c_str();
-        parsed = true;
-        type = STRING_TYPE;
-      }
-
-      inline void setBool(bool value) {
-        iValue = value;
-        parsed = true;
-        type = BOOL_TYPE;
-      }
-
-      inline void setUnparsedString(const std::string &value) {
-        sValue = value.c_str();
-        parsed = false;
-        type = UNDEFINED_TYPE;
-      }
-
-      inline std::string toString() const {
-        char text[64];
-
-        if(type == UNDEFINED_TYPE) {
-          return sValue;
-        } else if(type == INT_TYPE) {
-          sprintf(text, "%d", iValue);
-          return text;
-        } else if(type == UINT_TYPE) {
-          sprintf(text, "%u", uValue);
-          return text;
-        } else if(type == DOUBLE_TYPE) {
-          sprintf(text, "%g", dValue);
-          return text;
-        } else if(type == ULONG_TYPE) {
-          sprintf(text, "%lu", luValue);
-          return text;
-        } else if(type == STRING_TYPE) {
-          return sValue;
-        } else if(type == BOOL_TYPE) {
-          if(iValue) return "true";
-          else return "false";
-        }
-        return std::string();
-      }
-
-      inline bool hasKey(const std::string &key) const {
-        return (children.find(key) != children.end());
-      }
-
-      ConfigMap::iterator beginMap() {
-        return children.begin();
-      }
-
-      ConfigMap::iterator endMap() {
-        return children.end();
-      }
-
-      ConfigMap::iterator find(const std::string &key) {
-        return children.find(key);
-      }
-
-      size_t size() const {
-        return children.size();
-      }
-
-      bool isMap() const {
-        return (children.size() > 0);
-      }
-
-      bool isAtom() const {
-        return (children.size() == 0);
-      }
-
-      bool isVector() const {
-        return false;
-      }
-
-    private:
-      unsigned long luValue;
-      int iValue;
-      unsigned int uValue;
-      double dValue;
-      std::string sValue;
-      std::string parentName;
-      bool parsed;
-      ItemType type;
-
-      inline bool parseInt() {
-        if(type != UNDEFINED_TYPE) {
-          throw std::runtime_error("ConfigItem parsing wrong type line ...");
-        }
-        return parsed = sscanf(sValue.c_str(), "%d", &iValue);
-      }
-
-      inline bool parseUInt() {
-        if(type != UNDEFINED_TYPE) {
-          throw std::runtime_error("ConfigItem parsing wrong type line ...");
-        }
-        return parsed = sscanf(sValue.c_str(), "%u", &uValue);
-      }
-
-      inline bool parseULong() {
-        if(type != UNDEFINED_TYPE) {
-          throw std::runtime_error("ConfigItem parsing wrong type line ...");
-        }
-        return parsed = sscanf(sValue.c_str(), "%lu", &luValue);
-      }
-
-      inline bool parseDouble() {
-        if(type != UNDEFINED_TYPE) {
-          throw std::runtime_error("ConfigItem parsing wrong type line ...");
-        }
-        return parsed = sscanf(sValue.c_str(), "%lf", &dValue);
-      }
-
-      inline bool parseString() {
-        if(type != UNDEFINED_TYPE) {
-          throw std::runtime_error("ConfigItem parsing wrong type line ...");
-        }
-        return parsed = true;
-      }
-
-      inline bool parseBool() {
-        if(type != UNDEFINED_TYPE) {
-          throw std::runtime_error("ConfigItem parsing wrong type line ...");
-        }
-        sValue = trim(sValue);
-        if(sValue == "true" || sValue == "True" || sValue == "TRUE") {
-          iValue = 1;
-          return parsed = true;
-        }
-
-        if(sValue == "false" || sValue == "False" || sValue == "FALSE") {
-          iValue = 0;
-          return parsed = true;
-        }
-
-        return parsed = sscanf(sValue.c_str(), "%d", &iValue);
-      }
-
-    }; // end of class ConfigItem
-
-    typedef ConfigItem ConfigAtom;
-
-  } // end of namespace configmaps
-
-#endif // CONFIG_DATA_H
+// MZ: renamed "src/ConfigData.h" to "src/ConfigData.hpp" at 03.03.2016
+//
+// keep this file around for a month or so, then delete it
+#warning "do not include this file, use 'ConfigData.hpp' instead"
+#include "ConfigData.hpp"
diff --git a/src/ConfigData.hpp b/src/ConfigData.hpp
new file mode 100644
index 0000000..9208e94
--- /dev/null
+++ b/src/ConfigData.hpp
@@ -0,0 +1,732 @@
+/*
+ *  Copyright 2011, 2012, DFKI GmbH Robotics Innovation Center
+ *
+ *  This file is part of the MARS simulation framework.
+ *
+ *  MARS is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU Lesser General Public License
+ *  as published by the Free Software Foundation, either version 3
+ *  of the License, or (at your option) any later version.
+ *
+ *  MARS is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Lesser General Public License for more details.
+ *
+ *   You should have received a copy of the GNU Lesser General Public License
+ *   along with MARS.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef CONFIG_DATA_HPP
+#define CONFIG_DATA_HPP
+
+#ifdef _PRINT_HEADER_
+  #warning "ConfigData.hpp"
+#endif
+
+#include <string>
+#include <stdexcept>
+#include <vector>
+#include <cstdio>
+#include <iosfwd>
+
+#include "FIFOMap.hpp"
+
+namespace configmaps {
+
+    // only functions used from misc.h
+    std::string getPathOfFile(const std::string &filename);
+    std::string trim(const std::string& str);
+
+    class ConfigMap;
+    class ConfigItem;
+
+    template <typename T>
+    class ConfigVectorTemplate : public std::vector<T>{
+    public:
+
+      ConfigVectorTemplate(std::string s) : parentName(s) {}
+      ConfigVectorTemplate() : parentName("") {}
+
+      T& operator[](short index) {
+        return (*this)[(unsigned long)index];
+      }
+
+      T& operator[](unsigned short index) {
+        return (*this)[(unsigned long)index];
+      }
+
+      T& operator[](unsigned long index) {
+        if(index == this->size()) {
+          this->push_back(T());
+          this->back().setParentName(parentName);
+        }
+        return std::vector<T>::operator[](index);
+      }
+
+      T& operator[](long index) {
+        return (*this)[(unsigned long)index];
+      }
+
+      T& operator[](int index) {
+        return (*this)[(unsigned long)index];
+      }
+
+      T& operator[](unsigned int index) {
+        return (*this)[(unsigned long)index];
+      }
+
+      const T& operator[](unsigned long index) const {
+        return std::vector<T>::operator[](index);
+      }
+
+       ConfigVectorTemplate& operator[](const char *text) {
+        if(0 == this->size()) {
+          this->push_back(T());
+          this->back().setParentName(parentName);
+        }
+        return std::vector<T>::operator[](0)[text];
+      }
+
+      const ConfigVectorTemplate& operator[](const char *text) const {
+        return std::vector<T>::operator[](0)[text];
+      }
+
+      bool isVector() const {
+        return (this->size() > 1);
+      }
+
+      bool isMap() const {
+        if(this->size() == 1 && std::vector<T>::operator[](0).children.size() > 0) return true;
+        return false;
+      }
+
+      bool isAtom() const {
+        return (this->size() == 1 && std::vector<T>::operator[](0).children.size() == 0);
+      }
+
+      const char* c_str() {
+        return std::vector<T>::operator[](0).getString().c_str();
+      }
+
+      template <typename T1>
+      ConfigVectorTemplate& operator<<(const T1 &v) {
+        return (std::vector<T>::operator[](0) = v);
+      }
+
+      size_t append(const T &item) {
+        this->push_back(item);
+        this->back().setParentName(parentName);
+        return this->size() - 1;
+      }
+
+      ConfigVectorTemplate& operator+=(const T &item) {
+        this->push_back(item);
+        this->back().setParentName(parentName);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator=(const T &item) {
+        this->clear();
+        this->push_back(item);
+        this->back().setParentName(parentName);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator=(int v) {
+        *this = T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator=(bool v) {
+        *this = T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator=(unsigned int v) {
+        *this = T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator=(double v) {
+        *this = T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator=(unsigned long v) {
+        *this = T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator=(std::string v) {
+        *this = T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator=(const char* v) {
+        *this = T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator=(const ConfigMap &v) {
+        *this = T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator+=(const int& v) {
+        *this += T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator+=(const bool& v) {
+        *this += T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator+=(const unsigned int& v) {
+        *this += T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator+=(const double &v) {
+        *this += T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator+=(const unsigned long& v) {
+        *this += T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator+=(const std::string& v) {
+        *this += T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator+=(const char* v) {
+        *this += T(v);
+        return *this;
+      }
+
+      ConfigVectorTemplate& operator+=(const ConfigMap &v) {
+        *this += T(v);
+        return *this;
+      }
+
+      FIFOMap<std::string, ConfigVectorTemplate<ConfigItem> >::iterator beginMap() {
+        return (*this)[0].children.begin();
+      }
+
+      FIFOMap<std::string, ConfigVectorTemplate<ConfigItem> >::iterator endMap() {
+        return (*this)[0].children.end();
+      }
+
+      FIFOMap<std::string, ConfigVectorTemplate<ConfigItem> >::iterator find(const std::string &key) {
+        return (*this)[0].children.find(key);
+      }
+
+      bool hasKey(const std::string &key) {
+        return (*this)[0].children.hasKey(key);
+      }
+
+      std::string toString() {
+        return (*this)[0].toString();
+      }
+
+      operator ConfigMap & () {
+        return (*this)[0].children;
+      }
+
+      operator ConfigMap * () {
+        return &(*this)[0].children;
+      }
+
+      operator ConfigItem & () {
+        return (*this)[0];
+      }
+
+      operator ConfigItem * () {
+        return &(*this)[0];
+      }
+
+      operator int () {
+        return (*this)[0].getInt();
+      }
+
+      operator double () {
+        return (*this)[0].getDouble();
+      }
+
+      operator unsigned int () {
+        return (*this)[0].getUInt();
+      }
+
+      operator unsigned long () {
+        return (*this)[0].getULong();
+      }
+
+      operator std::string () {
+        return (std::string)(*this)[0].getString().c_str();
+      }
+
+      std::string getString() {
+        return (std::string)(*this)[0].getString().c_str();
+      }
+
+      operator bool () {
+        return (*this)[0].getBool();
+      }
+
+      inline void setParentName(std::string s) {
+        parentName = s;
+      }
+
+      inline const std::string& getParentName() const {
+        return parentName;
+      }
+
+    private:
+      std::string parentName;
+
+    }; // end of class ConfigVectorTemplate
+
+    class ConfigItem;
+
+    // these typedefs are here for two reasons.
+    // 1) they make the types nicer to use
+    // 2) this enables us to use a ConfigMap inside the ConfigItem!
+    typedef ConfigVectorTemplate<ConfigItem> ConfigVector;
+
+    template <typename T>
+    T& operator<<(T &a, ConfigVector& v) {
+      return (a = (T)(v[0]));
+    }
+
+    class ConfigMap : public FIFOMap<std::string, ConfigVector> {
+    public:
+      ConfigVector& operator[](const std::string &name) {
+        if(find(name) == end()) {
+          return FIFOMap<std::string, ConfigVector>::operator[](name) = ConfigVector(name);
+        }
+        return FIFOMap<std::string, ConfigVector>::operator[](name);
+      }
+
+      void toYamlStream(std::ostream &out) const;
+      void toYamlFile(const std::string &filename) const;
+      std::string toYamlString() const;
+      static ConfigMap fromYamlStream(std::istream &in);
+      static ConfigMap fromYamlFile(const std::string &filename,
+                                    bool loadURI = false);
+      static ConfigMap fromYamlString(const std::string &s);
+      static void recursiveLoad(ConfigMap *map, std::string path);
+
+      bool hasKey(const std::string &key) const {
+        return (find(key) != end());
+      }
+
+      // checks if the key is in the list, if not return the given default value
+      template<typename T> T get(const std::string &key,
+                                 const T &defaultValue) {
+        if(find(key) != end()) {
+          return (T)(*this)[key];
+        }
+        return defaultValue;
+      }
+
+      // checks if the key is in the list, if not add the given default value
+      // and return it
+      template<typename T> T getOrCreate(const std::string &key,
+                                         const T &defaultValue) {
+        if(find(key) != end()) {
+          return (T)(*this)[key];
+        }
+        (*this)[key] = defaultValue;
+        return defaultValue;
+      }
+
+    };
+
+    // todo: support vector and quaternion
+    // a vector has three children (x, y, z) which can be parsed
+
+    class ConfigItem {
+    public:
+      ConfigMap children;
+      enum ItemType {UNDEFINED_TYPE, INT_TYPE, UINT_TYPE, DOUBLE_TYPE,
+                     ULONG_TYPE, STRING_TYPE, BOOL_TYPE};
+
+      ConfigItem() : luValue(0), iValue(0), uValue(0), dValue(0.0),
+                     parsed(false), type(UNDEFINED_TYPE) {}
+
+      ConfigItem(int val) : luValue(0), iValue(val),
+                                     uValue(0), dValue(0.0),
+                                     parsed(true), type(INT_TYPE) {}
+
+       ConfigItem(bool val) : luValue(0), iValue(val),
+                                      uValue(0), dValue(0.0),
+                                      parsed(true), type(BOOL_TYPE) {}
+
+       ConfigItem(unsigned int val) : luValue(0), iValue(0),
+                                              uValue(val), dValue(0.0),
+                                              parsed(true), type(UINT_TYPE) {}
+
+       ConfigItem(double val) : luValue(0), iValue(0),
+                                        uValue(0), dValue(val),
+                                        parsed(true), type(DOUBLE_TYPE) {}
+
+       ConfigItem(unsigned long val) : luValue(val), iValue(0),
+                                               uValue(0), dValue(0.0),
+                                               parsed(true), type(ULONG_TYPE) {}
+
+       ConfigItem(std::string val) : luValue(0), iValue(0),
+                                             uValue(0), dValue(0.0),
+                                             sValue(val.c_str()), parsed(false),
+                                             type(UNDEFINED_TYPE) {}
+
+       ConfigItem(const char *val) : luValue(0), iValue(0),
+                                             uValue(0), dValue(0.0),
+                                             sValue(val), parsed(false),
+                                             type(UNDEFINED_TYPE) {}
+
+       ConfigItem(ConfigMap map) : children(map), luValue(0), iValue(0),
+                                           uValue(0), dValue(0.0),
+                                           sValue(""), parsed(false),
+                                           type(UNDEFINED_TYPE) {}
+
+      ConfigVector& operator[](const std::string &name) {
+        return children[name];
+      }
+
+      ConfigVector& operator[](const char* name) {
+        return children[name];
+      }
+
+      operator ConfigMap& () {
+        return children;
+      }
+
+      operator ConfigMap* () {
+        return &children;
+      }
+
+      operator ConfigVector& () {
+        throw 1;
+      }
+
+      operator ConfigVector* () {
+        throw 1;
+      }
+
+      operator int () {
+        return getInt();
+      }
+
+      operator double () {
+        return getDouble();
+      }
+
+      operator unsigned int () {
+        return getUInt();
+      }
+
+      operator unsigned long () {
+        return getULong();
+      }
+
+      operator std::string () {
+        return (std::string)getString().c_str();
+      }
+
+      operator bool () {
+        return getBool();
+      }
+
+      inline ItemType getType() const {
+        return type;
+      }
+
+
+      inline void setParentName(std::string s) {
+        parentName = s;
+      }
+
+      inline const std::string& getParentName() const {
+        return parentName;
+      }
+
+      inline bool testType(ItemType _type) {
+        if(type == UNDEFINED_TYPE) {
+          switch(_type) {
+          case INT_TYPE:
+            return parseInt();
+          case DOUBLE_TYPE:
+            return parseDouble();
+          case UINT_TYPE:
+            return parseUInt();
+          case ULONG_TYPE:
+            return parseULong();
+          case STRING_TYPE:
+            return parseString();
+          case BOOL_TYPE:
+            return parseBool();
+          default:
+            return false;
+          }
+        }
+        else if(type == _type) return true;
+        return false;
+      }
+
+      inline int getInt() {
+        if(type != UNDEFINED_TYPE && type != INT_TYPE) {
+          char text[50];
+          sprintf(text, "ConfigItem parsing wrong type getInt: %s - %s",
+                  parentName.c_str(), sValue.c_str());
+          throw std::runtime_error(text);
+        }
+
+        if(!parsed) parseInt();
+        if(parsed) type = INT_TYPE;
+
+        return iValue;
+      }
+
+      inline double getDouble() {
+        if(type != UNDEFINED_TYPE && type != DOUBLE_TYPE) {
+          char text[50];
+          sprintf(text, "ConfigItem parsing wrong type getDouble: %s - %s",
+                  parentName.c_str(), sValue.c_str());
+          throw std::runtime_error(text);
+        }
+        if(!parsed) parseDouble();
+        if(parsed) type = DOUBLE_TYPE;
+        return dValue;
+      }
+
+      inline unsigned int getUInt() {
+        if(type != UNDEFINED_TYPE && type != UINT_TYPE) {
+          char text[50];
+          sprintf(text, "ConfigItem parsing wrong type getInt: %s - %s",
+                  parentName.c_str(), sValue.c_str());
+          throw std::runtime_error(text);
+        }
+
+        if(!parsed) parseUInt();
+        if(parsed) type = UINT_TYPE;
+
+        return uValue;
+      }
+
+      inline unsigned long getULong() {
+        if(type != UNDEFINED_TYPE && type != ULONG_TYPE) {
+          char text[50];
+          sprintf(text, "ConfigItem parsing wrong type getULong: %s - %s",
+                  parentName.c_str(), sValue.c_str());
+          throw std::runtime_error(text);
+        }
+        if(!parsed) parseULong();
+        if(parsed) type = ULONG_TYPE;
+        return luValue;
+      }
+
+      inline std::string getString() {
+        if(type != UNDEFINED_TYPE && type != STRING_TYPE) {
+          char text[50];
+          sprintf(text, "ConfigItem parsing wrong type getString: %s - %s",
+                  parentName.c_str(), sValue.c_str());
+          throw std::runtime_error(text);
+        }
+        if(!parsed) parseString();
+        if(parsed) type = STRING_TYPE;
+        return sValue.c_str();
+      }
+
+      inline std::string getUnparsedString() const {
+        return sValue.c_str();
+      }
+
+      inline int getBool() {
+        if(type != UNDEFINED_TYPE && type != BOOL_TYPE) {
+          char text[50];
+          sprintf(text, "ConfigItem parsing wrong type getBool: %s - %s",
+                  parentName.c_str(), sValue.c_str());
+          throw std::runtime_error(text);
+        }
+
+        if(!parsed) parseBool();
+        if(parsed) type = BOOL_TYPE;
+
+        return iValue;
+      }
+
+      inline void setInt(int value) {
+        iValue = value;
+        parsed = true;
+        type = INT_TYPE;
+      }
+
+      inline void setDouble(double value) {
+        dValue = value;
+        parsed = true;
+        type = DOUBLE_TYPE;
+      }
+
+      inline void setUInt(unsigned int value) {
+        uValue = value;
+        parsed = true;
+        type = UINT_TYPE;
+      }
+
+      inline void setULong(unsigned long value) {
+        luValue = value;
+        parsed = true;
+        type = ULONG_TYPE;
+      }
+
+      inline void setString(const std::string &value) {
+        sValue = value.c_str();
+        parsed = true;
+        type = STRING_TYPE;
+      }
+
+      inline void setBool(bool value) {
+        iValue = value;
+        parsed = true;
+        type = BOOL_TYPE;
+      }
+
+      inline void setUnparsedString(const std::string &value) {
+        sValue = value.c_str();
+        parsed = false;
+        type = UNDEFINED_TYPE;
+      }
+
+      inline std::string toString() const {
+        char text[64];
+
+        if(type == UNDEFINED_TYPE) {
+          return sValue;
+        } else if(type == INT_TYPE) {
+          sprintf(text, "%d", iValue);
+          return text;
+        } else if(type == UINT_TYPE) {
+          sprintf(text, "%u", uValue);
+          return text;
+        } else if(type == DOUBLE_TYPE) {
+          sprintf(text, "%g", dValue);
+          return text;
+        } else if(type == ULONG_TYPE) {
+          sprintf(text, "%lu", luValue);
+          return text;
+        } else if(type == STRING_TYPE) {
+          return sValue;
+        } else if(type == BOOL_TYPE) {
+          if(iValue) return "true";
+          else return "false";
+        }
+        return std::string();
+      }
+
+      inline bool hasKey(const std::string &key) const {
+        return (children.find(key) != children.end());
+      }
+
+      ConfigMap::iterator beginMap() {
+        return children.begin();
+      }
+
+      ConfigMap::iterator endMap() {
+        return children.end();
+      }
+
+      ConfigMap::iterator find(const std::string &key) {
+        return children.find(key);
+      }
+
+      size_t size() const {
+        return children.size();
+      }
+
+      bool isMap() const {
+        return (children.size() > 0);
+      }
+
+      bool isAtom() const {
+        return (children.size() == 0);
+      }
+
+      bool isVector() const {
+        return false;
+      }
+
+    private:
+      unsigned long luValue;
+      int iValue;
+      unsigned int uValue;
+      double dValue;
+      std::string sValue;
+      std::string parentName;
+      bool parsed;
+      ItemType type;
+
+      inline bool parseInt() {
+        if(type != UNDEFINED_TYPE) {
+          throw std::runtime_error("ConfigItem parsing wrong type line ...");
+        }
+        return parsed = sscanf(sValue.c_str(), "%d", &iValue);
+      }
+
+      inline bool parseUInt() {
+        if(type != UNDEFINED_TYPE) {
+          throw std::runtime_error("ConfigItem parsing wrong type line ...");
+        }
+        return parsed = sscanf(sValue.c_str(), "%u", &uValue);
+      }
+
+      inline bool parseULong() {
+        if(type != UNDEFINED_TYPE) {
+          throw std::runtime_error("ConfigItem parsing wrong type line ...");
+        }
+        return parsed = sscanf(sValue.c_str(), "%lu", &luValue);
+      }
+
+      inline bool parseDouble() {
+        if(type != UNDEFINED_TYPE) {
+          throw std::runtime_error("ConfigItem parsing wrong type line ...");
+        }
+        return parsed = sscanf(sValue.c_str(), "%lf", &dValue);
+      }
+
+      inline bool parseString() {
+        if(type != UNDEFINED_TYPE) {
+          throw std::runtime_error("ConfigItem parsing wrong type line ...");
+        }
+        return parsed = true;
+      }
+
+      inline bool parseBool() {
+        if(type != UNDEFINED_TYPE) {
+          throw std::runtime_error("ConfigItem parsing wrong type line ...");
+        }
+        sValue = trim(sValue);
+        if(sValue == "true" || sValue == "True" || sValue == "TRUE") {
+          iValue = 1;
+          return parsed = true;
+        }
+
+        if(sValue == "false" || sValue == "False" || sValue == "FALSE") {
+          iValue = 0;
+          return parsed = true;
+        }
+
+        return parsed = sscanf(sValue.c_str(), "%d", &iValue);
+      }
+
+    }; // end of class ConfigItem
+
+    typedef ConfigItem ConfigAtom;
+
+  } // end of namespace configmaps
+
+#endif // CONFIG_DATA_HPP
diff --git a/src/FIFOMap.h b/src/FIFOMap.h
index 1b897a5..c6e021a 100644
--- a/src/FIFOMap.h
+++ b/src/FIFOMap.h
@@ -1,244 +1,5 @@
-/*
- *  Copyright 2012, DFKI GmbH Robotics Innovation Center
- *
- *  This file is part of the MARS simulation framework.
- *
- *  MARS is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU Lesser General Public License
- *  as published by the Free Software Foundation, either version 3
- *  of the License, or (at your option) any later version.
- *
- *  MARS is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU Lesser General Public License for more details.
- *
- *   You should have received a copy of the GNU Lesser General Public License
- *   along with MARS.  If not, see <http://www.gnu.org/licenses/>.
- *
- */
-
-#ifndef MARS_UTILS_FIFO_MAP_H
-#define MARS_UTILS_FIFO_MAP_H
-
-#ifdef _PRINT_HEADER_
-#warning "FIFOMap.h"
-#endif
-
-#include <map>
-#include <list>
-#include <iterator>
-#include <iostream>
-#include <algorithm>
-
-namespace configmaps {
-
-    template <typename Key, typename T>
-    class FIFOItem {
-    public:
-      FIFOItem(const Key &k, T &val)
-        : first(k), second(val)
-      {}
-      bool operator==(const FIFOItem &other) const {
-        return first == other.first;
-      }
-
-      Key first;
-      T &second;
-    }; // end of class FIFOItem
-
-    template <typename Key, typename T>
-    class FIFOMap : public std::map<Key, T> {
-    public:
-      typedef typename std::map<Key, T>::iterator mapIterator;
-      typedef typename std::map<Key, T>::const_iterator const_mapIterator;
-      typedef typename std::list<FIFOItem<Key, T> >::iterator iterator;
-      typedef typename std::list<FIFOItem<Key, T> >::const_iterator const_iterator;
-
-      /* iterator stuff */
-      iterator begin()
-      { return insertOrder.begin(); }
-      const_iterator begin() const
-      { return insertOrder.begin(); }
-      iterator end()
-      { return insertOrder.end(); }
-      const_iterator end() const
-      { return insertOrder.end(); }
-
-      /* ctor, copy-ctor and assignment operator */
-      FIFOMap() {}
-      FIFOMap(const FIFOMap<Key, T> &other)
-      { *this = other; }
-      FIFOMap<Key, T>& operator=(const FIFOMap<Key, T> &other);
-
-      /* element access */
-      T& operator[](const Key &x);
-
-      /* modifieres */
-      std::pair<iterator, bool> insert(const std::pair<const Key, T> &x);
-
-      /* not implemented
-         iterator insert(iterator position, const value_type &x);
-         template <class InputIterator>
-         void insert ( InputIterator first, InputIterator last );
-      */
-    
-      void erase(iterator position);
-      size_t erase(const Key &x);
-      void erase(iterator first, iterator last);
-      void swap( FIFOMap<Key, T> &other);
-      void clear();
-
-      void append(FIFOMap<Key, T> &other);
-      void append(const FIFOMap<Key, T> &other);
-
-      /* operations */
-      iterator find(const Key &x);
-      const_iterator find(const Key &x) const
-      { return const_iterator(const_cast<FIFOMap*>(this)->find(x)); }
-
-      /* not implemented yet;
-         iterator lower_bound ( const key_type& x );
-         const_iterator lower_bound ( const key_type& x ) const;
-         iterator upper_bound ( const key_type& x );
-         const_iterator upper_bound ( const key_type& x ) const;
-         pair<iterator,iterator> equal_range ( const key_type& x );
-         pair<const_iterator,const_iterator> equal_range ( const key_type& x ) const;
-      */
-
-    private:
-      std::list<FIFOItem<Key, T> > insertOrder;
-
-    }; // end of class FIFOMap
-
-
-    /*********************
-     * Implementation    *
-     *********************/
-
-    template<typename Key, typename T>
-    FIFOMap<Key, T>& FIFOMap<Key, T>::operator=(const FIFOMap<Key, T> &other) {
-      if(this == &other)
-        return *this;
-      clear();
-      std::map<Key, T>::operator=(other);
-      for(const_iterator it = other.begin(); it != other.end(); ++it) {
-        FIFOItem<Key, T> newItem(it->first, 
-                                 std::map<Key, T>::operator[](it->first));
-        insertOrder.push_back(newItem);
-      }
-      return *this;
-    }
-
-    /* element access */
-    template<typename Key, typename T>
-    T& FIFOMap<Key, T>::operator[](const Key &x) {
-      mapIterator it = std::map<Key, T>::find(x);
-      if(it != std::map<Key, T>::end()) {
-        return it->second;
-      } else {
-        FIFOItem<Key, T> newItem(x, std::map<Key, T>::operator[](x));
-        insertOrder.push_back(newItem);
-        return newItem.second;
-      }
-    }
-
-    /* modifieres */
-    template<typename Key, typename T>
-    std::pair<typename FIFOMap<Key, T>::iterator, bool> FIFOMap<Key, T>::insert(const std::pair<const Key, T> &x) {
-      std::cerr << "FIFOMap::insert is untested" << std::endl;
-      mapIterator it = this->find(x.first);
-      if(it != std::map<Key, T>::end()) {
-        return std::make_pair(std::find(insertOrder.begin(), 
-                                        insertOrder.end(), 
-                                        FIFOItem<Key, T>(it->first,
-                                                         it->second)), 
-                              false);
-      } else {
-        std::pair<mapIterator, bool> tmp;
-        tmp = std::map<Key, T>::insert(x);
-        insertOrder.push_back(FIFOItem<Key, T>(x.first, tmp.first->second));
-        return std::make_pair(--insertOrder.end(), true);
-      }
-    }
-    
-    template<typename Key, typename T>
-    void FIFOMap<Key, T>::erase(FIFOMap<Key, T>::iterator position) {
-      // seems to work fine
-      // std::cerr << "FIFOMap::erase is untested" << std::endl;
-      std::map<Key, T>::erase(position->first);
-      insertOrder.erase(position);
-    }
-
-    template<typename Key, typename T>
-    size_t FIFOMap<Key, T>::erase(const Key &x) {
-      std::cerr << "FIFOMap::erase is untested" << std::endl;
-      size_t ret = std::map<Key, T>::erase(x);
-      if(ret) {
-        for(iterator it = begin(); it != end(); ++it) {
-          if(it->first == x) {
-            insertOrder.erase(it);
-            break;
-          }
-        }
-      }
-      return ret;
-    }
-
-    template<typename Key, typename T>
-    void FIFOMap<Key, T>::erase(FIFOMap::iterator first,
-                                FIFOMap::iterator last) {
-      std::cerr << "FIFOMap::erase is untested" << std::endl;
-      for(iterator it = first; it != last; /* do nothing */) {
-        std::map<Key, T>::erase(it->first);
-        it = insertOrder.erase(it);
-      }
-    }
-
-    template<typename Key, typename T>
-    void FIFOMap<Key, T>::swap(FIFOMap<Key, T> &other) {
-      std::cerr << "FIFOMap::swap is untested" << std::endl;
-      std::map<Key, T>::swap(other);
-      insertOrder.swap(other.insertOrder);
-    }
-
-    template<typename Key, typename T>
-    void FIFOMap<Key, T>::append(FIFOMap<Key, T> &other) {
-      //FIFOMap<Key, T>::iterator it = other.begin();
-      iterator it = other.begin();
-      for(; it!=other.end(); ++it) {
-        FIFOMap<Key, T>::operator[](it->first) = it->second;
-      }
-    }
-
-    template<typename Key, typename T>
-    void FIFOMap<Key, T>::append(const FIFOMap<Key, T> &other) {
-      //FIFOMap<Key, T>::const_iterator it = other.begin();
-      const_iterator it = other.begin();
-      for(; it!=other.end(); ++it) {
-        FIFOMap<Key, T>::operator[](it->first) = it->second;
-      }
-    }
-
-    template<typename Key, typename T>
-    void FIFOMap<Key, T>::clear() {
-      std::map<Key, T>::clear();
-      insertOrder.clear();
-    }
-    
-    /* operations */
-    template<typename Key, typename T>
-    typename FIFOMap<Key, T>::iterator FIFOMap<Key, T>::find(const Key &x) {
-      typename std::map<Key, T>::iterator it = std::map<Key, T>::find(x);
-      if(it != std::map<Key, T>::end()) {
-        return std::find(insertOrder.begin(), 
-                         insertOrder.end(), 
-                         FIFOItem<Key, T>(it->first, 
-                                          it->second));
-      }
-      return end();
-    }
-    
-} // end of namespace configmaps
-
-#endif /* MARS_UTILS_FIFO_MAP_H */
+// MZ: renamed "src/FIFOMap.h" to "src/FIFOMap.hpp" at 03.03.2016
+//
+// keep this file around for a month or so, then delete it
+#warning "do not include this file, use 'FIFOMap.hpp' instead"
+#include "FIFOMap.hpp"
diff --git a/src/FIFOMap.hpp b/src/FIFOMap.hpp
new file mode 100644
index 0000000..e70f6e2
--- /dev/null
+++ b/src/FIFOMap.hpp
@@ -0,0 +1,244 @@
+/*
+ *  Copyright 2012, DFKI GmbH Robotics Innovation Center
+ *
+ *  This file is part of the MARS simulation framework.
+ *
+ *  MARS is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU Lesser General Public License
+ *  as published by the Free Software Foundation, either version 3
+ *  of the License, or (at your option) any later version.
+ *
+ *  MARS is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Lesser General Public License for more details.
+ *
+ *   You should have received a copy of the GNU Lesser General Public License
+ *   along with MARS.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef MARS_UTILS_FIFO_MAP_HPP
+#define MARS_UTILS_FIFO_MAP_HPP
+
+#ifdef _PRINT_HEADER_
+#warning "FIFOMap.hpp"
+#endif
+
+#include <map>
+#include <list>
+#include <iterator>
+#include <iostream>
+#include <algorithm>
+
+namespace configmaps {
+
+    template <typename Key, typename T>
+    class FIFOItem {
+    public:
+      FIFOItem(const Key &k, T &val)
+        : first(k), second(val)
+      {}
+      bool operator==(const FIFOItem &other) const {
+        return first == other.first;
+      }
+
+      Key first;
+      T &second;
+    }; // end of class FIFOItem
+
+    template <typename Key, typename T>
+    class FIFOMap : public std::map<Key, T> {
+    public:
+      typedef typename std::map<Key, T>::iterator mapIterator;
+      typedef typename std::map<Key, T>::const_iterator const_mapIterator;
+      typedef typename std::list<FIFOItem<Key, T> >::iterator iterator;
+      typedef typename std::list<FIFOItem<Key, T> >::const_iterator const_iterator;
+
+      /* iterator stuff */
+      iterator begin()
+      { return insertOrder.begin(); }
+      const_iterator begin() const
+      { return insertOrder.begin(); }
+      iterator end()
+      { return insertOrder.end(); }
+      const_iterator end() const
+      { return insertOrder.end(); }
+
+      /* ctor, copy-ctor and assignment operator */
+      FIFOMap() {}
+      FIFOMap(const FIFOMap<Key, T> &other)
+      { *this = other; }
+      FIFOMap<Key, T>& operator=(const FIFOMap<Key, T> &other);
+
+      /* element access */
+      T& operator[](const Key &x);
+
+      /* modifieres */
+      std::pair<iterator, bool> insert(const std::pair<const Key, T> &x);
+
+      /* not implemented
+         iterator insert(iterator position, const value_type &x);
+         template <class InputIterator>
+         void insert ( InputIterator first, InputIterator last );
+      */
+    
+      void erase(iterator position);
+      size_t erase(const Key &x);
+      void erase(iterator first, iterator last);
+      void swap( FIFOMap<Key, T> &other);
+      void clear();
+
+      void append(FIFOMap<Key, T> &other);
+      void append(const FIFOMap<Key, T> &other);
+
+      /* operations */
+      iterator find(const Key &x);
+      const_iterator find(const Key &x) const
+      { return const_iterator(const_cast<FIFOMap*>(this)->find(x)); }
+
+      /* not implemented yet;
+         iterator lower_bound ( const key_type& x );
+         const_iterator lower_bound ( const key_type& x ) const;
+         iterator upper_bound ( const key_type& x );
+         const_iterator upper_bound ( const key_type& x ) const;
+         pair<iterator,iterator> equal_range ( const key_type& x );
+         pair<const_iterator,const_iterator> equal_range ( const key_type& x ) const;
+      */
+
+    private:
+      std::list<FIFOItem<Key, T> > insertOrder;
+
+    }; // end of class FIFOMap
+
+
+    /*********************
+     * Implementation    *
+     *********************/
+
+    template<typename Key, typename T>
+    FIFOMap<Key, T>& FIFOMap<Key, T>::operator=(const FIFOMap<Key, T> &other) {
+      if(this == &other)
+        return *this;
+      clear();
+      std::map<Key, T>::operator=(other);
+      for(const_iterator it = other.begin(); it != other.end(); ++it) {
+        FIFOItem<Key, T> newItem(it->first, 
+                                 std::map<Key, T>::operator[](it->first));
+        insertOrder.push_back(newItem);
+      }
+      return *this;
+    }
+
+    /* element access */
+    template<typename Key, typename T>
+    T& FIFOMap<Key, T>::operator[](const Key &x) {
+      mapIterator it = std::map<Key, T>::find(x);
+      if(it != std::map<Key, T>::end()) {
+        return it->second;
+      } else {
+        FIFOItem<Key, T> newItem(x, std::map<Key, T>::operator[](x));
+        insertOrder.push_back(newItem);
+        return newItem.second;
+      }
+    }
+
+    /* modifieres */
+    template<typename Key, typename T>
+    std::pair<typename FIFOMap<Key, T>::iterator, bool> FIFOMap<Key, T>::insert(const std::pair<const Key, T> &x) {
+      std::cerr << "FIFOMap::insert is untested" << std::endl;
+      mapIterator it = this->find(x.first);
+      if(it != std::map<Key, T>::end()) {
+        return std::make_pair(std::find(insertOrder.begin(), 
+                                        insertOrder.end(), 
+                                        FIFOItem<Key, T>(it->first,
+                                                         it->second)), 
+                              false);
+      } else {
+        std::pair<mapIterator, bool> tmp;
+        tmp = std::map<Key, T>::insert(x);
+        insertOrder.push_back(FIFOItem<Key, T>(x.first, tmp.first->second));
+        return std::make_pair(--insertOrder.end(), true);
+      }
+    }
+    
+    template<typename Key, typename T>
+    void FIFOMap<Key, T>::erase(FIFOMap<Key, T>::iterator position) {
+      // seems to work fine
+      // std::cerr << "FIFOMap::erase is untested" << std::endl;
+      std::map<Key, T>::erase(position->first);
+      insertOrder.erase(position);
+    }
+
+    template<typename Key, typename T>
+    size_t FIFOMap<Key, T>::erase(const Key &x) {
+      std::cerr << "FIFOMap::erase is untested" << std::endl;
+      size_t ret = std::map<Key, T>::erase(x);
+      if(ret) {
+        for(iterator it = begin(); it != end(); ++it) {
+          if(it->first == x) {
+            insertOrder.erase(it);
+            break;
+          }
+        }
+      }
+      return ret;
+    }
+
+    template<typename Key, typename T>
+    void FIFOMap<Key, T>::erase(FIFOMap::iterator first,
+                                FIFOMap::iterator last) {
+      std::cerr << "FIFOMap::erase is untested" << std::endl;
+      for(iterator it = first; it != last; /* do nothing */) {
+        std::map<Key, T>::erase(it->first);
+        it = insertOrder.erase(it);
+      }
+    }
+
+    template<typename Key, typename T>
+    void FIFOMap<Key, T>::swap(FIFOMap<Key, T> &other) {
+      std::cerr << "FIFOMap::swap is untested" << std::endl;
+      std::map<Key, T>::swap(other);
+      insertOrder.swap(other.insertOrder);
+    }
+
+    template<typename Key, typename T>
+    void FIFOMap<Key, T>::append(FIFOMap<Key, T> &other) {
+      //FIFOMap<Key, T>::iterator it = other.begin();
+      iterator it = other.begin();
+      for(; it!=other.end(); ++it) {
+        FIFOMap<Key, T>::operator[](it->first) = it->second;
+      }
+    }
+
+    template<typename Key, typename T>
+    void FIFOMap<Key, T>::append(const FIFOMap<Key, T> &other) {
+      //FIFOMap<Key, T>::const_iterator it = other.begin();
+      const_iterator it = other.begin();
+      for(; it!=other.end(); ++it) {
+        FIFOMap<Key, T>::operator[](it->first) = it->second;
+      }
+    }
+
+    template<typename Key, typename T>
+    void FIFOMap<Key, T>::clear() {
+      std::map<Key, T>::clear();
+      insertOrder.clear();
+    }
+    
+    /* operations */
+    template<typename Key, typename T>
+    typename FIFOMap<Key, T>::iterator FIFOMap<Key, T>::find(const Key &x) {
+      typename std::map<Key, T>::iterator it = std::map<Key, T>::find(x);
+      if(it != std::map<Key, T>::end()) {
+        return std::find(insertOrder.begin(), 
+                         insertOrder.end(), 
+                         FIFOItem<Key, T>(it->first, 
+                                          it->second));
+      }
+      return end();
+    }
+    
+} // end of namespace configmaps
+
+#endif /* MARS_UTILS_FIFO_MAP_HPP */