Refactoring of MMM framework to SimoxUtility: Added string helpers and...

Refactoring of MMM framework to SimoxUtility: Added string helpers and conversion tools, a distinguishable color lookup table e.g. for creating graphs, a rapidxml wrapper for easier parsing of xml files and moved deprecated RobotPoseDiffIK from MMMTools to VirtualRobot

CMakeLists.txt

@@ -76,7 +76,7 @@ if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 8.0)
     add_compile_options(-Wno-error=unused-but-set-variable)
     add_compile_options(-Wno-error=unused-variable)
     add_compile_options(-Wno-error=unused-parameter)
-    add_compile_options(-Wno-error=deprecated)
+    add_compile_options(-Wno-error=deprecated-declarations)
 endif()
 
 #######################################################################################
@@ -89,12 +89,13 @@ OPTION(Simox_USE_OPENSCENEGRAPH_VISUALIZATION "Use OpenSceneGraph for visualizat
 OPTION(Simox_COLLISION_CHECKER_PQP "Build and use the PQP Collision Detection library (located in ExternalDependencies)" ON)
 #######################################################################################
 MESSAGE(STATUS "******************** Configuring Simox ************************")
+set (Simox_BUILD_VirtualRobot TRUE CACHE BOOL "Build Virtual Robot library")
 set (Simox_BUILD_Saba TRUE CACHE BOOL "Build Motion Planning library")
 set (Simox_BUILD_GraspStudio TRUE CACHE BOOL "Build Grasp Planning library")
 set (Simox_BUILD_SimDynamics TRUE CACHE BOOL "Build Dynamic Simulation")
 
 MESSAGE (STATUS "** SIMOX BUILD SimoxUtility: TRUE (can't be turned off)")
-MESSAGE (STATUS "** SIMOX BUILD VirtualRobot: TRUE (can't be turned off)")
+MESSAGE (STATUS "** SIMOX BUILD VirtualRobot: ${Simox_BUILD_VirtualRobot}")
 MESSAGE (STATUS "** SIMOX BUILD Saba        : ${Simox_BUILD_Saba}")
 MESSAGE (STATUS "** SIMOX BUILD GraspStudio : ${Simox_BUILD_GraspStudio}")
 MESSAGE (STATUS "** SIMOX BUILD SimDynamics : ${Simox_BUILD_SimDynamics}")
@@ -194,13 +195,15 @@ endif()
 
 #######################################################################################
 add_subdirectory(SimoxUtility)
-add_subdirectory(VirtualRobot)
-
+list (APPEND SIMOX_EXPORT_TARGET_LIST SimoxUtility)
+list (APPEND Simox_LIBRARIES          SimoxUtility)
 
 #######################################################################################
-list (APPEND SIMOX_EXPORT_TARGET_LIST SimoxUtility VirtualRobot)
-list (APPEND Simox_LIBRARIES          SimoxUtility VirtualRobot)
-
+if (Simox_BUILD_VirtualRobot)
+    add_subdirectory(VirtualRobot)
+    list(APPEND SIMOX_EXPORT_TARGET_LIST VirtualRobot)
+    list(APPEND Simox_LIBRARIES VirtualRobot)
+endif()
 
 #######################################################################################
 if (Simox_BUILD_Saba)

SimoxUtility/CMakeLists.txt

@@ -50,6 +50,8 @@ MESSAGE (STATUS "\n***** CONFIGURING Simox project SimoxUtility *****")
 SET(SOURCES
     SimoxUtility.cpp
 
+    algorithm/string/string_tools.cpp
+
     color/cmaps/colormaps.cpp
     color/cmaps/Named.cpp
     color/Color.cpp
@@ -89,6 +91,9 @@ SET(INCLUDES
     algorithm/for_each_if.h
     algorithm/apply.hpp
     algorithm/get_map_keys_values.h
+    algorithm/string/string_tools.h
+    algorithm/string/string_conversion.h
+    algorithm/string/string_conversion_eigen.h
 
     color/cmaps/colormaps.h
     color/cmaps/Named.h
@@ -96,6 +101,7 @@ SET(INCLUDES
     color/Color.h
     color/ColorMap.h
     color/GlasbeyLUT.h
+    color/KellyLUT.h
     color/hsv.h
     color/interpolation.h
     color/json.h
@@ -106,6 +112,7 @@ SET(INCLUDES
 
     error/SimoxError.h
 
+    filesystem/make_relative.h
     filesystem/list_directory.h
     filesystem/remove_trailing_separator.h
 
@@ -216,6 +223,11 @@ SET(INCLUDES
     meta/enum/adapt_enum.h
     meta/enum/EnumNames.hpp
 
+    xml/rapidxml/rapidxml.hpp
+    xml/rapidxml/rapidxml_print.hpp
+    xml/rapidxml/RapidXMLWrapper.h
+    xml/rapidxml/RapidXMLForwardDecl.h
+
     shapes/AxisAlignedBoundingBox.h
     shapes/OrientedBoxBase.h
     shapes/OrientedBox.h

SimoxUtility/algorithm.h

@@ -4,3 +4,4 @@
 
 #include "algorithm/for_each_if.h"
 #include "algorithm/get_map_keys_values.h"
+#include "algorithm/string.h"

SimoxUtility/algorithm/string.h

+#pragma once
+
+// This file is generated!
+
+#include "string/string_conversion.h"
+#include "string/string_conversion_eigen.h"
+#include "string/string_tools.h"

SimoxUtility/algorithm/string/string_conversion.h

+/**
+ * This file is part of Simox.
+ *
+ * Simox 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 2 of
+ * the License, or (at your option) any later version.
+ *
+ * Simox 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 this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Andre Meixner (andre dot meixner at kit dot edu)
+ * @copyright  2020 Andre Meixner
+ *             GNU Lesser General Public License
+ */
+
+#pragma once
+
+#include <type_traits>
+#include <iomanip>
+
+#include "SimoxUtility/error.h"
+#include "string_tools.h"
+
+namespace simox::alg {
+    namespace help {
+        template <typename T>
+        struct type{};
+
+        template<typename T, typename std::enable_if<std::is_fundamental<T>::value>::type* = nullptr>
+        inline T to_(const std::string& s, type<T>, std::locale locale = DEFAULT_LOCALE, bool trim = true)
+        {
+            char c;
+            std::stringstream ss;
+            ss.imbue(locale);
+            if (trim)
+                ss << alg::trim_copy(s);
+            else
+                ss << s;
+            T value;
+            ss >> value;
+            if (ss.fail() || ss.get(c)) {
+                throw error::SimoxError("Cannot convert string " + s + " to " + typeid(T).name());
+                ss.clear();
+            }
+            return value;
+        }
+
+        template<typename T, typename std::enable_if<std::is_fundamental<T>::value>::type* = nullptr>
+        inline T to_(const std::string& s, type<bool>, std::locale /*locale*/, bool trim = true)
+        {
+            std::string help = to_lower(s);
+            if (trim) alg::trim(help);
+            if (help == "true" || help == "1") return true;
+            else if (help == "false" || help == "0") return false;
+            else throw error::SimoxError("Cannot convert string " + s + " to boolean.");
+        }
+    }
+
+    template<typename T, typename std::enable_if<std::is_fundamental<T>::value>::type* = nullptr>
+    inline T to_(const std::string& s, std::locale locale = DEFAULT_LOCALE, bool trim = true)
+    {
+        return help::to_<T>(s, help::type<T>{}, locale, trim);
+    }
+
+    template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
+    inline std::vector<T> to_vec(const std::vector<std::string>& vec, std::locale locale = DEFAULT_LOCALE, bool trimElements = false)
+    {
+        std::vector<T> res;
+        for (const std::string& str : vec)
+        {
+            res.push_back(to_<T>(str, locale, trimElements));
+        }
+        return res;
+    }
+
+    template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
+    inline std::vector<T> to_vec(const std::string& str, const std::string& splitBy = "\t ", bool trimElements = true,
+                                 bool ignoreEmptyElements = true, std::locale locale = DEFAULT_LOCALE)
+    {
+        return to_vec<T>(split(str, splitBy, trimElements, ignoreEmptyElements, locale), locale, false);
+    }
+
+    template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
+    inline std::vector<T> to_vec_check_size(const std::string& str, unsigned int expectedSize, const std::string& splitBy = "\t ",
+                                       bool trimElements = true, bool ignoreEmptyElements = true, std::locale locale = DEFAULT_LOCALE)
+    {
+        return to_vec<T>(split_check_size(str, expectedSize, splitBy, trimElements, ignoreEmptyElements, locale), locale, false);
+    }
+
+
+
+    template<typename T, typename std::enable_if<std::is_fundamental<T>::value>::type* = nullptr>
+    inline std::string to_string(T x, const std::locale& locale = DEFAULT_LOCALE)
+    {
+        std::stringstream ss;
+        ss.imbue(locale);
+        ss << x;
+        return ss.str();
+    }
+
+    template<> inline std::string to_string<double>(double d,  const std::locale& locale)
+    {
+        std::stringstream ss;
+        ss.imbue(locale);
+        ss << std::setprecision(12) << d;
+        return ss.str();
+    }
+
+    template<> inline std::string to_string<bool>(bool x, const std::locale& /*locale*/)
+    {
+        return x ? "true" : "false";
+    }
+
+    template<typename T, typename std::enable_if<std::is_fundamental<T>::value>::type* = nullptr>
+    inline std::string to_string(const std::vector<T>& vec, const std::string& splitBy = " ", std::locale locale = DEFAULT_LOCALE)
+    {
+        std::stringstream stream;
+         stream.imbue(locale);
+        for(unsigned int i = 0; i < vec.size(); ++i) {
+            stream << to_string(vec.at(i));
+            if (i + 1 != vec.size()) stream << splitBy;
+        }
+        return stream.str();
+    }
+
+    template<typename T, typename std::enable_if<!std::is_fundamental<T>::value>::type* = nullptr>
+    inline std::string to_string(const std::vector<T>& vec, const std::string& splitBy = " ", std::locale locale = DEFAULT_LOCALE)
+    {
+        std::stringstream stream;
+        stream.imbue(locale);
+        for(unsigned int i = 0; i < vec.size(); ++i) {
+            stream << vec.at(i);
+            if (i + 1 != vec.size()) stream << splitBy;
+        }
+        return stream.str();
+    }
+}

SimoxUtility/algorithm/string/string_conversion_eigen.h

+/**
+ * This file is part of Simox.
+ *
+ * Simox 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 2 of
+ * the License, or (at your option) any later version.
+ *
+ * Simox 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 this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Andre Meixner (andre dot meixner at kit dot edu)
+ * @copyright  2020 Andre Meixner
+ *             GNU Lesser General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+
+#include "string_conversion.h"
+
+namespace simox::alg {
+    template<typename T, int Rows = Eigen::Dynamic, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
+    inline Eigen::Matrix<T, Rows, 1> to_eigen(const std::vector<std::string>& vec, std::locale locale = DEFAULT_LOCALE, bool trimElements = false)
+    {
+        if (Rows != Eigen::Dynamic && Rows != (int)vec.size())
+            throw error::SimoxError("Sizes do not match!");
+        Eigen::Matrix<T, Rows, 1> res(vec.size());
+        for (unsigned int i = 0; i < vec.size(); i++)
+        {
+            res(i) = to_<T>(vec[i], locale, trimElements);
+        }
+        return res;
+    }
+
+    template<typename T = float, typename std::enable_if<std::is_scalar<T>::value>::type* = nullptr>
+    inline Eigen::Matrix<T, Eigen::Dynamic, 1> to_eigen_vec(const std::string& str, const std::string& splitBy = "\t ",
+                                                            bool trimElements = true, bool ignoreEmptyElements = true, std::locale locale = DEFAULT_LOCALE)
+    {
+        return to_eigen<T>(split(str, splitBy, trimElements, ignoreEmptyElements, locale), locale, false);
+    }
+
+    template<typename T = float, typename std::enable_if<std::is_scalar<T>::value>::type* = nullptr>
+    inline Eigen::Matrix<T, Eigen::Dynamic, 1> to_eigen_vec_check_rows(const std::string& str, int expectedRows, const std::string& splitBy = "\t ",
+                                                                       bool trimElements = true, bool ignoreEmptyElements = true, std::locale locale = DEFAULT_LOCALE)
+    {
+        return to_eigen<T>(split_check_size(str, expectedRows, splitBy, trimElements, ignoreEmptyElements, locale), locale, false);
+    }
+
+    template<typename T, unsigned int Rows, typename std::enable_if<std::is_scalar<T>::value>::type* = nullptr>
+    inline Eigen::Matrix<T, Rows, 1> to_eigen_vec(const std::string& str, const std::string& splitBy = "\t ",
+                                                  bool trimElements = true, bool ignoreEmptyElements = true, std::locale locale = DEFAULT_LOCALE)
+    {
+        return to_eigen<T, Rows>(split_check_size(str, Rows, splitBy, trimElements, ignoreEmptyElements, locale), locale, false);
+    }
+
+    template<typename T, int Rows, typename std::enable_if<std::is_arithmetic<T>::value>::type* = nullptr>
+    inline std::string to_string(const Eigen::Matrix<T, Rows, 1>& vec, const std::string& splitBy = " ", std::locale locale = DEFAULT_LOCALE)
+    {
+        std::ostringstream stream;
+        stream.imbue(locale);
+        for(unsigned int i = 0; i < vec.rows(); ++i) {
+            stream << to_string(vec(i));
+            if (i + 1 != vec.rows()) stream << splitBy;
+        }
+        return stream.str();
+    }
+}

SimoxUtility/algorithm/string/string_tools.cpp

+#include "string_tools.h"
+
+#include "SimoxUtility/error/SimoxError.h"
+
+#include <algorithm>
+
+#include <iomanip>
+#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
+
+namespace simox::alg
+{
+
+std::string to_lower(const std::string& str)
+{
+    std::string res = str;
+    std::transform(res.begin(), res.end(), res.begin(), tolower);
+    return res;
+}
+
+void trim(std::string& str, const std::locale& locale) {
+    boost::trim(str, locale);
+}
+
+std::string trim_copy(const std::string& str, const std::locale& locale) {
+    return boost::trim_copy(str, locale);
+}
+
+void trim_if(std::string& str, const std::string& trim) {
+    boost::trim_if(str, boost::is_any_of(trim));
+}
+
+std::string trim_copy_if(std::string& str, const std::string& trim) {
+    return boost::trim_copy_if(str, boost::is_any_of(trim));
+}
+
+std::vector<std::string> split(const std::string& str, const std::string& splitBy, bool trimElements, bool removeEmptyElements, const std::locale& locale)
+{
+    std::vector<std::string> strs;
+    boost::split(strs, str, boost::is_any_of(splitBy));
+
+    if (trimElements) std::for_each(strs.begin(), strs.end(), boost::bind(&boost::trim<std::string>, _1, locale));
+
+    if (removeEmptyElements) strs.erase(std::remove_if(strs.begin(), strs.end(), [](const std::string& s) { return s.empty(); }), strs.end());
+
+    return strs;
+}
+
+std::vector<std::string> split_check_size(const std::string& str, unsigned int expectedSize, const std::string& splitBy, bool trimElements, bool removeEmptyElements, const std::locale& locale)
+{
+    std::vector<std::string> strs = split(str, splitBy, trimElements, removeEmptyElements, locale);
+
+    if (strs.size() != expectedSize)
+        throw error::SimoxError("String " + str + " contains " + std::to_string(strs.size()) + " instead of " + std::to_string(expectedSize) + " values seperated by delimiter " + splitBy);
+    return strs;
+}
+
+std::string join(const std::vector<std::string> vec, const std::string& delimiter, bool trimElements, bool ignoreEmptyElements, const std::locale& locale) {
+    std::ostringstream ss;
+    ss.imbue(locale);
+    for (size_t index = 0; index < vec.size(); index++)
+    {
+        if (trimElements)
+        {
+            std::string trimmed = trim_copy(vec.at(index), locale);
+            if (ignoreEmptyElements && trimmed.empty()) continue;
+            ss << trimmed;
+        }
+        else
+        {
+            if (ignoreEmptyElements && vec.at(index).empty()) continue;
+            ss << vec.at(index);
+        }
+        if (index != vec.size()) ss << delimiter;
+    }
+    return ss.str();
+}
+
+}

SimoxUtility/algorithm/string/string_tools.h

+/**
+ * This file is part of Simox.
+ *
+ * Simox 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 2 of
+ * the License, or (at your option) any later version.
+ *
+ * Simox 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 this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Andre Meixner (andre dot meixner at kit dot edu)
+ * @copyright  2020 Andre Meixner
+ *             GNU Lesser General Public License
+ */
+
+#pragma once
+
+#include <string>
+#include <vector>
+#include <locale>
+
+namespace simox::alg {
+    static std::locale DEFAULT_LOCALE = std::locale::classic();
+
+    std::string to_lower(const std::string& str);
+
+    void trim(std::string& str, const std::locale& locale = DEFAULT_LOCALE);
+
+    std::string trim_copy(const std::string& str, const std::locale& locale = DEFAULT_LOCALE);
+
+    void trim_if(std::string& str, const std::string& trim = "\t ");
+
+    std::string trim_copy_if(const std::string& str, const std::string& trim = "\t ");
+
+    std::vector<std::string> split(const std::string& str, const std::string& splitBy = "\t ", bool trimElements = true,
+                                   bool removeEmptyElements = true, const std::locale& locale = DEFAULT_LOCALE);
+
+    //! @param expectedSize throws SimoxError if split not matches expectedSize
+    std::vector<std::string> split_check_size(const std::string& str, unsigned int expectedSize, const std::string& splitBy = "\t ",
+                                         bool trimElements = true, bool removeEmptyElements = true, const std::locale& locale = DEFAULT_LOCALE);
+
+    std::string join(const std::vector<std::string> vec, const std::string& delimiter = " ", bool trimElements = false,
+                     bool ignoreEmptyElements = false, const std::locale& locale = DEFAULT_LOCALE);
+}

SimoxUtility/color.h

@@ -5,6 +5,7 @@
 #include "color/Color.h"
 #include "color/ColorMap.h"
 #include "color/GlasbeyLUT.h"
+#include "color/KellyLUT.h"
 #include "color/cmaps.h"
 #include "color/hsv.h"
 #include "color/interpolation.h"

SimoxUtility/color/KellyLUT.h

+/*
+This file is part of MMM.
+
+MMM is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+MMM 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 General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with MMM.  If not, see <http://www.gnu.org/licenses/>.
+*
+* @package    MMM
+* @author     Andre Meixner
+* @copyright  2020 High Performance Humanoid Technologies (H2T, 255; Karlsruhe, Germany
+*
+*/
+
+#pragma once
+
+#include "Color.h"
+#include <vector>
+#include <set>
+
+namespace simox::color
+{
+
+/**
+ * @brief 20 distinguishable colors by Kenneth L. Kelly but without black and white, e.g. usable for creating graphs
+ */
+class KellyLUT {
+public:
+    static const inline Color kelly_vivid_yellow          =   Color(0xFF, 0xB3, 0x00, 255);
+    static const inline Color kelly_strong_purple         =   Color(0x80, 0x3E, 0x75, 255);
+    static const inline Color kelly_vivid_orange          =   Color(0xFF, 0x68, 0x00, 255);
+    static const inline Color kelly_very_light_blue       =   Color(0xA6, 0xBD, 0xD7, 255);
+    static const inline Color kelly_vivid_red             =   Color(0xC1, 0x00, 0x20, 255);
+    static const inline Color kelly_grayish_yellow        =   Color(0xCE, 0xA2, 0x62, 255);
+    static const inline Color kelly_medium_gray           =   Color(0x81, 0x70, 0x66, 255);
+
+    //The following will not be good for people with defective color vision
+    static const inline Color kelly_vivid_green           =   Color(0x00, 0x7D, 0x34, 255);
+    static const inline Color kelly_strong_purplish_pink  =   Color(0xF6, 0x76, 0x8E, 255);
+    static const inline Color kelly_strong_blue           =   Color(0x00, 0x53, 0x8A, 255);
+    static const inline Color kelly_yellowish_pink        =   Color(0xFF, 0x7A, 0x5C, 255);
+    static const inline Color kelly_strong_violet         =   Color(0x53, 0x37, 0x7A, 255);
+    static const inline Color kelly_vivid_orange_yellow   =   Color(0xFF, 0x8E, 0x00, 255);
+    static const inline Color kelly_strong_purplish_red   =   Color(0xB3, 0x28, 0x51, 255);
+    static const inline Color kelly_vivid_greenish_yellow =   Color(0xF4, 0xC8, 0x00, 255);
+    static const inline Color kelly_strong_reddish_brown  =   Color(0x7F, 0x18, 0x0D, 255);
+    static const inline Color kelly_vivid_yellowish_green =   Color(0x93, 0xAA, 0x00, 255);
+    static const inline Color kelly_deep_yellowish_brown  =   Color(0x59, 0x33, 0x15, 255);
+    static const inline Color kelly_vivid_reddish_orange  =   Color(0xF1, 0x3A, 0x13, 255);
+    static const inline Color kelly_dark_olive_green      =   Color(0x23, 0x2C, 0x16, 255);
+
+    static const inline std::vector<Color> KELLY_COLORS_COLOR_BLIND =
+    {
+        kelly_vivid_yellow, kelly_strong_purple, kelly_vivid_orange, kelly_very_light_blue, kelly_vivid_red, kelly_grayish_yellow, kelly_medium_gray
+    };
+
+    static const inline std::vector<Color> KELLY_COLORS =
+    {
+        kelly_vivid_yellow, kelly_strong_purple, kelly_vivid_orange, kelly_very_light_blue, kelly_vivid_red, kelly_grayish_yellow, kelly_medium_gray,
+        kelly_vivid_green, kelly_strong_purplish_pink, kelly_strong_blue, kelly_strong_violet, kelly_strong_violet, kelly_vivid_orange_yellow,
+        kelly_strong_purplish_red, kelly_vivid_greenish_yellow, kelly_strong_reddish_brown, kelly_vivid_yellowish_green, kelly_deep_yellowish_brown,
+        kelly_vivid_reddish_orange, kelly_dark_olive_green
+    };
+
+    /**
+     * @brief Get a color from the lookup table with given ID.
+     * The ID is automaticall wrapped if greater than `size()`.
+     */
+    static Color at(std::size_t id, int alpha = 255, bool color_blind = false)
+    {
+        id = id % size(color_blind);
+        return data(color_blind).at(id).with_alpha(alpha);
+    }
+
+    /**
+     * @brief Get a color from the lookup table with given ID (with float values).
+     * The ID is automaticall wrapped if greater than `size()`.
+     */
+    static Eigen::Vector4f atf(std::size_t id, float alpha = 1.f, bool color_blind = false)
+    {
+        return at(id, to_byte(alpha), color_blind).to_vector4f();
+    }
+
+    /**
+     * @brief Get a color from the lookup table with given ID.
+     * The ID is automaticall wrapped if greater than `size()`.
+     */
+    template <typename UIntT, std::enable_if_t<std::is_unsigned_v<UIntT>, int> = 0>
+    static Color at(UIntT id, int alpha = 255, bool color_blind = false)
+    {
+        return at(static_cast<std::size_t>(id), alpha, color_blind);
+    }
+    /**
+     * @brief Get a color from the lookup table with given ID.
+     * The ID is automaticall wrapped if greater than `size()`.
+     * If `id` is negative, its absolute value is used.
+     */
+    template <typename IntT, std::enable_if_t<std::is_signed_v<IntT>, int> = 0>
+    static Color at(IntT id, int alpha = 255, bool color_blind = false)
+    {
+        return at(static_cast<std::size_t>(id >= 0 ? id : std::abs(id)), alpha, color_blind);
+    }
+
+    /**
+     * @brief Get a color from the lookup table with given ID (with float values).
+     * The ID is automaticall wrapped if greater than `size()`.
+     */
+    template <typename UIntT, std::enable_if_t<std::is_unsigned_v<UIntT>, int> = 0>
+    static Eigen::Vector4f atf(UIntT id, float alpha = 1.f, bool color_blind = false)
+    {
+        return atf(static_cast<std::size_t>(id), alpha, color_blind);
+    }
+
+    /**
+     * @brief Get a color from the lookup table with given ID (with float values).
+     * The ID is automaticall wrapped if greater than `size()`.
+     * If `id` is negative, its absolute value is used.
+     */
+    template <typename IntT, std::enable_if_t<std::is_signed_v<IntT>, int> = 0>
+    static Eigen::Vector4f atf(IntT id, float alpha = 1.f, bool color_blind = false)
+    {
+        return atf(static_cast<std::size_t>(id >= 0 ? id : std::abs(id)), alpha, color_blind);
+    }
+
+    /// Get the number of colors in the lookup table.;
+    static std::size_t size(bool color_blind = false)
+    {
+        return data(color_blind).size();
+    }
+
+    static std::vector<Color> data(bool color_blind = false)
+    {
+        return color_blind ? KELLY_COLORS_COLOR_BLIND : KELLY_COLORS;
+    }
+};
+
+
+/**
+ * @brief An RGBA color, where each component is a byte in [0, 255]. Additionally contains an id denoting its position
+ */
+struct KellyColor : public Color
+{
+    KellyColor(size_t id = -1) : KellyColor(Color(), id)
+    {
+    }
+
+    KellyColor(uint8_t r, uint8_t g, uint8_t b, size_t id) : Color(r,g,b), id(id)
+    {
+    }
+
+    KellyColor(Color color, size_t id) : KellyColor(color.r, color.g, color.b, id)
+    {
+    }
+
+    bool operator <(const KellyColor &other) const
+    {
+       return id < other.id;
+    }
+
+    size_t id;
+};
+
+/**
+ * @brief Stack of 20 distinguishable colors by Kenneth L. Kelly but without black and white, e.g. usable for creating graphs
+ */
+class KellyColorStack {
+public:
+    /**
+     * @brief Initializes a Stack of colors
+     * @param color_blind 7 instead of 20 colors for visually impaired
+     */
+    KellyColorStack(bool color_blind = false) :
+        colors(createColorStack(KellyLUT::data(color_blind)))
+    {
+    }
+
+    /**
+     * @brief Returns the next Kelly color and removes it from the stack, black color if stack is empty
+     * @return The next color from the top of the stack
+     */
+    KellyColor next()
+    {
+        if (!colors.empty())
+        {
+            auto it = colors.begin();
+            auto color = *it;
+            colors.erase(it);
+            return color;
+        }
+        else return KellyColor();
+    }
+
+    /**
+     * @brief Puts a color back on the stack depending on its id
+     */
+    void putBack(KellyColor color)
+    {
+        colors.insert(color);
+    }
+
+    /**
+     * @return Amount of colors left on the stack
+     */
+    std::size_t left()
+    {
+        return colors.size();
+    }
+
+    bool empty()
+    {
+        return colors.empty();
+    }
+
+private:
+    inline std::set<KellyColor> createColorStack(const std::vector<Color> &colors)
+    {
+        std::set<KellyColor> set;
+        size_t index = 0;
+        for (const Color &color : colors)
+        {
+            set.insert(KellyColor(color, index++));
+        }
+        return set;
+    }
+
+    std::set<KellyColor> colors;
+};
+
+}

SimoxUtility/filesystem.h

@@ -3,4 +3,5 @@
 // This file is generated!
 
 #include "filesystem/list_directory.h"
+#include "filesystem/make_relative.h"
 #include "filesystem/remove_trailing_separator.h"

SimoxUtility/filesystem/make_relative.h

+#pragma once
+
+#include <filesystem>
+
+namespace simox::fs
+{
+    //! Return path when appended to a_From will resolve to same as a_To
+    inline std::filesystem::path make_relative(std::filesystem::path a_From, std::filesystem::path a_To)
+    {
+        a_From = std::filesystem::absolute(a_From);
+        a_To = std::filesystem::absolute(a_To);
+        std::filesystem::path ret;
+        std::filesystem::path::const_iterator itrFrom(a_From.begin()), itrTo(a_To.begin());
+        // Find common base
+        for (std::filesystem::path::const_iterator toEnd(a_To.end()), fromEnd(a_From.end()); itrFrom != fromEnd && itrTo != toEnd && *itrFrom == *itrTo; ++itrFrom, ++itrTo);
+        // Navigate backwards in directory to reach previously found base
+        for (std::filesystem::path::const_iterator fromEnd(a_From.end()); itrFrom != fromEnd; ++itrFrom)
+        {
+            if ((*itrFrom) != ".")
+                ret /= "..";
+        }
+        // Now navigate down the directory branch
+        //ret.append(itrTo, a_To.end());
+        for (; itrTo != a_To.end(); ++itrTo)
+            ret /= *itrTo;
+        return ret;
+    }
+}

SimoxUtility/rapidxml.h

+#pragma once
+
+// This file is generated!
+
+#include "rapidxml/RapidXMLWrapper.h"

SimoxUtility/tests/CMakeLists.txt

-
 SET(TEST_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR})
 
 MACRO(ADD_SU_TEST TEST_NAME)
@@ -19,6 +18,7 @@ MACRO(ADD_SU_TEST TEST_NAME)
 
     ADD_EXECUTABLE(${TRG_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/${TEST_NAME}.cpp)
     TARGET_LINK_LIBRARIES(${TRG_NAME} PRIVATE SimoxUtility Boost::unit_test_framework)
+    target_include_directories(${TRG_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/..)
     if (NOT Boost_USE_STATIC_LIBS)
         target_compile_definitions(${TRG_NAME} PRIVATE -DBOOST_TEST_DYN_LINK)
     endif ()

SimoxUtility/tests/algorithm/CMakeLists.txt

 
 ADD_SU_TEST( apply )
 ADD_SU_TEST( for_each_if )
+ADD_SU_TEST( string_tools )
 

SimoxUtility/tests/algorithm/string_tools.cpp

+/**
+* @package    SimoxUtility
+* @author     Andre Meixner
+* @copyright  2020 Andre Meixner
+*/
+
+#define BOOST_TEST_MODULE SimoxUtility/algorithm/string_tools
+
+#include <boost/test/included/unit_test.hpp>
+
+#include <SimoxUtility/algorithm/string.h>
+
+BOOST_AUTO_TEST_CASE(to_lower)
+{
+    BOOST_CHECK_EQUAL(simox::alg::to_lower("TO LoWER tesT!"), "to lower test!");
+}
+
+BOOST_AUTO_TEST_CASE(split)
+{
+    std::string testString = "   test       20 50   30 ";
+    std::string testString2 = "   test   ;   20,50 ; 30 ";
+    std::vector<std::string> expectedResult = { "test", "20", "50", "30" };
+    BOOST_CHECK(simox::alg::split(testString) == expectedResult);
+    BOOST_CHECK(simox::alg::split(testString2, ";,") == expectedResult);
+    BOOST_CHECK_THROW(simox::alg::split_check_size(testString, 3), simox::error::SimoxError);
+}
+
+BOOST_AUTO_TEST_CASE(to_eigen)
+{
+    std::string testString = " 5.0   3   -1.0   ";
+    Eigen::Vector3f expectedResult(5.0, 3.0, -1.0);
+    BOOST_CHECK(simox::alg::to_eigen_vec(testString) == expectedResult);
+    BOOST_CHECK((simox::alg::to_eigen_vec<float, 3>(testString)) == expectedResult);
+    BOOST_CHECK_THROW((simox::alg::to_eigen_vec<float, 4>(testString)), simox::error::SimoxError);
+}
+
+BOOST_AUTO_TEST_CASE(to_string)
+{
+    bool testBool = false;
+    float testFloat = -3.156;
+    double testDouble = -3.34919295196;
+    Eigen::Vector3f testVector3f(5.0, 3.5, -1.0);
+    Eigen::VectorXf testVectorXf(9);
+    testVectorXf << 5.0, 3.5, -1.0, 15, 20, 10, 30, 100, 1;
+    std::vector<float> testVector = {5.0, 3.5, -1.0};
+    std::vector<int> testVectorI = {1, -2, 3};
+    std::vector<std::string> testVectorStr = {"Hello", "World!"};
+    BOOST_CHECK_EQUAL(simox::alg::to_string(testBool), "false");
+    BOOST_CHECK_EQUAL(simox::alg::to_string(testFloat), "-3.156");
+    BOOST_CHECK_EQUAL(simox::alg::to_string(testDouble), "-3.34919295196");
+    BOOST_CHECK_EQUAL(simox::alg::to_string(testVector3f), "5 3.5 -1");
+    BOOST_CHECK_EQUAL(simox::alg::to_string(testVectorXf), "5 3.5 -1 15 20 10 30 100 1");
+    BOOST_CHECK_EQUAL(simox::alg::to_string(testVector), "5 3.5 -1");
+    BOOST_CHECK_EQUAL(simox::alg::to_string(testVectorI, ";"), "1;-2;3");
+    BOOST_CHECK_EQUAL(simox::alg::to_string(testVectorStr), "Hello World!");
+
+}
+
+BOOST_AUTO_TEST_CASE(from_string)
+{
+    std::string testBool = " 0";
+    std::string testBool2 = "FALSE";
+    std::string testFloat = " -3.156";
+    std::string testVector = " 5 3.5 -1.0 ";
+    std::string testVector2 = " 5;3.5;-1.0 ";
+    std::string testVector3 = " 5 ,3.5 ,-1 ,15 ,20, 10,30,100,1 ";
+    bool expectedBool = false;
+    float expectedFloat = -3.156f;
+    Eigen::Vector3f expectedVector3f(5.0, 3.5, -1.0);
+    Eigen::VectorXd expectedVectorXd(9);
+    expectedVectorXd << 5.0, 3.5, -1.0, 15, 20, 10, 30, 100, 1;
+    Eigen::VectorXd expectedVector9d(9);
+    expectedVector9d << 5.0, 3.5, -1.0, 15, 20, 10, 30, 100, 1;
+    std::vector<float> expectedVector = {5.0, 3.5, -1.0};
+    BOOST_CHECK_EQUAL(simox::alg::to_<bool>(testBool), expectedBool);
+    BOOST_CHECK_EQUAL(simox::alg::to_<bool>(testBool2), expectedBool);
+    BOOST_CHECK_EQUAL(simox::alg::to_<float>(testFloat), expectedFloat);
+    BOOST_CHECK(simox::alg::to_vec<float>(testVector) == expectedVector);
+    BOOST_CHECK(simox::alg::to_vec_check_size<float>(testVector2, 3, ";") == expectedVector);
+    BOOST_CHECK((simox::alg::to_eigen_vec<float, 3>(testVector)) == expectedVector3f);
+    BOOST_CHECK(simox::alg::to_eigen_vec(testVector2, ";") == expectedVector3f);
+    BOOST_CHECK(simox::alg::to_eigen_vec<double>(testVector3, ",") == expectedVectorXd);
+    BOOST_CHECK((simox::alg::to_eigen_vec_check_rows<double>(testVector3, 9, ",")) == expectedVectorXd);
+    BOOST_CHECK_THROW((simox::alg::to_eigen_vec_check_rows<double>(testVector3, 10, ",")), simox::error::SimoxError);
+    BOOST_CHECK((simox::alg::to_eigen_vec<double, 9>(testVector3, ",")) == expectedVector9d);
+}

SimoxUtility/tests/filesystem/CMakeLists.txt

 
 ADD_SU_TEST( list_directory_test )
+ADD_SU_TEST( make_relative )

SimoxUtility/tests/filesystem/make_relative.cpp

+/**
+* @package    VirtualRobot
+* @author     Andre Meixner
+* @copyright  2020 Andre Meixner
+*/
+
+#define BOOST_TEST_MODULE SimoxUtility/filesystem/make_relative
+
+#include <boost/test/included/unit_test.hpp>
+
+#include <SimoxUtility/filesystem/make_relative.h>
+
+namespace fs = simox::fs;
+
+BOOST_AUTO_TEST_CASE(make_relative)
+{
+    BOOST_CHECK_EQUAL(simox::fs::make_relative("a/b/c/e/", "a/b/d/f.xml"), "../../../d/f.xml");
+    auto path = std::filesystem::path("a/b/c/e/g.xml").parent_path();
+    BOOST_CHECK_EQUAL(simox::fs::make_relative(path, "a/b/d/f.xml"), "../../../d/f.xml");
+    BOOST_CHECK_EQUAL(simox::fs::make_relative("a/b", "a/d/f.xml"), "../d/f.xml");
+}

SimoxUtility/xml.h

+#pragma once
+
+// This file is generated!
+
+#include "xml/rapidxml.h"

SimoxUtility/xml/rapidxml.h

+#pragma once
+
+// This file is generated!
+
+#include "rapidxml/RapidXMLForwardDecl.h"
+#include "rapidxml/RapidXMLWrapper.h"