param_loader.hpp

// clang: MatousFormat
#pragma once
 
#include <mrs_lib/param_loader.h>
 
namespace mrs_lib
{
 
  /* printValue function and overloads //{ */
 
  template <typename T>
  void ParamLoader::printValue(const resolved_name_t& name, const T& value) const
  {
    if (m_node_name.empty())
      std::cout << "\t" << name << ":\t" << value << std::endl;
    else
      RCLCPP_INFO_STREAM(m_node->get_logger(), "[" << m_node_name << "]: parameter '" << name << "':\t" << value);
  }
 
  template <typename T>
  std::ostream& operator<<(std::ostream& os, const Eigen::MatrixX<T>& var)
  {
    /* const Eigen::IOFormat fmt(4, 0, ", ", "\n", "\t\t[", "]"); */
    /* strstr << value.format(fmt); */
    const Eigen::IOFormat fmt;
    return os << var.format(fmt);
  }
  //}
 
  /* loading helper functions //{ */
 
  /* helper functions for loading dynamic Eigen matrices //{ */
  // loadMatrixX helper function for loading dynamic Eigen matrices //{
  template <typename T>
  std::pair<ParamLoader::MatrixX<T>, bool> ParamLoader::loadMatrixX(const std::string& name, const MatrixX<T>& default_value, int rows, int cols,
                                                                    optional_t optional, unique_t unique, swap_t swap, bool printValues)
  {
    const auto resolved_name = m_pp.resolveName(name);
    MatrixX<T> loaded = default_value;
    bool used_rosparam_value = false;
    // first, check if the user already tried to load this parameter
    if (unique && check_duplicit_loading(resolved_name))
      return {loaded, used_rosparam_value};
 
    // this function only accepts dynamic columns (you can always transpose the matrix afterward)
    if (rows < 0)
    {
      // if the parameter was compulsory, alert the user and set the flag
      printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved_name.str);
      m_load_successful = false;
      return {loaded, used_rosparam_value};
    }
    const bool expect_zero_matrix = rows == 0;
    if (expect_zero_matrix)
    {
      if (cols > 0)
      {
        printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved_name.str
                   + ". One dimension indicates zero matrix, but other expects non-zero.");
        m_load_successful = false;
        return {loaded, used_rosparam_value};
      }
    }
 
    bool cur_load_successful = true;
    bool check_size_exact = true;
    if (cols <= 0) // this means that the cols dimension is dynamic or a zero matrix is expected
      check_size_exact = false;
 
    std::vector<T> tmp_vec;
    // try to load the parameter
    const bool success = m_pp.getParam(resolved_name, tmp_vec);
    // check if the loaded vector has correct length
    bool correct_size = (int)tmp_vec.size() == rows * cols;
    if (!check_size_exact && !expect_zero_matrix)
      correct_size = (int)tmp_vec.size() % rows == 0; // if the cols dimension is dynamic, the size just has to be divisable by rows
 
    if (success && correct_size)
    {
      // if successfully loaded, everything is in order
      // transform the vector to the matrix
      if (cols <= 0 && rows > 0)
        cols = tmp_vec.size() / rows;
      if (swap)
        std::swap(rows, cols);
      loaded = Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>, Eigen::Unaligned>(tmp_vec.data(), rows, cols);
      used_rosparam_value = true;
    } else
    {
      if (success && !correct_size)
      {
        // warn the user that this parameter was not successfully loaded because of wrong vector length (might be an oversight)
        std::string warning =
            std::string("Matrix parameter ") + resolved_name.str
            + std::string(" could not be loaded because the vector has a wrong length " + std::to_string(tmp_vec.size()) + " instead of expected ");
        // process the message correctly based on whether the loaded matrix should be dynamic or static
        if (cols <= 0) // for dynamic matrices
          warning = warning + std::string("number divisible by ") + std::to_string(rows);
        else // for static matrices
          warning = warning + std::to_string(rows * cols);
        printWarning(warning);
      }
      // if it was not loaded, the default value is used (set at the beginning of the function)
      if (!optional)
      {
        // if the parameter was compulsory, alert the user and set the flag
        printError(std::string("Could not load non-optional parameter ") + resolved_name.str);
        cur_load_successful = false;
      }
    }
 
    // check if load was a success
    if (cur_load_successful)
    {
      if (m_print_values && printValues)
        printValue(resolved_name, loaded);
      m_loaded_params.insert(resolved_name);
    } else
    {
      m_load_successful = false;
    }
    // finally, return the resulting value
    return {loaded, used_rosparam_value};
  }
  //}
 
  /* loadMatrixStatic_internal helper function for loading static Eigen matrices //{ */
  template <int rows, int cols, typename T>
  std::pair<Eigen::Matrix<T, rows, cols>, bool>
  ParamLoader::loadMatrixStatic_internal(const std::string& name, const Eigen::Matrix<T, rows, cols>& default_value, optional_t optional, unique_t unique)
  {
    const auto [dynamic, loaded_ok] = loadMatrixX(name, MatrixX<T>(default_value), rows, cols, optional, unique, NO_SWAP);
    return {dynamic, loaded_ok};
  }
  //}
 
  /* loadMatrixKnown_internal helper function for loading EigenXd matrices with known dimensions //{ */
  template <typename T>
  std::pair<ParamLoader::MatrixX<T>, bool> ParamLoader::loadMatrixKnown_internal(const std::string& name, const MatrixX<T>& default_value, int rows, int cols,
                                                                                 optional_t optional, unique_t unique)
  {
    MatrixX<T> loaded = default_value;
    // first, check that at least one dimension is set
    if (rows <= 0 || cols <= 0)
    {
      printError(std::string("Invalid expected matrix dimensions for parameter ") + m_pp.resolveName(name).str + std::string(" (use loadMatrixDynamic?)"));
      m_load_successful = false;
      return {loaded, false};
    }
 
    return loadMatrixX(name, default_value, rows, cols, optional, unique, NO_SWAP);
  }
  //}
 
  /* loadMatrixDynamic_internal helper function for loading Eigen matrices with one dynamic (unspecified) dimension //{ */
  template <typename T>
  std::pair<ParamLoader::MatrixX<T>, bool> ParamLoader::loadMatrixDynamic_internal(const std::string& name, const MatrixX<T>& default_value, int rows, int cols,
                                                                                   optional_t optional, unique_t unique)
  {
    MatrixX<T> loaded = default_value;
 
    // next, check that at least one dimension is set
    if (rows <= 0 && cols <= 0)
    {
      printError(std::string("Invalid expected matrix dimensions for parameter ") + m_pp.resolveName(name).str
                 + std::string(" (at least one dimension must be specified)"));
      m_load_successful = false;
      return {loaded, false};
    }
 
    swap_t swap = NO_SWAP;
    if (rows <= 0)
    {
      std::swap(rows, cols);
      swap = SWAP;
    }
    return loadMatrixX(name, default_value, rows, cols, optional, unique, swap);
  }
  //}
 
  /* loadMatrixArray_internal helper function for loading an array of EigenXd matrices with known dimensions //{ */
  template <typename T>
  std::vector<ParamLoader::MatrixX<T>> ParamLoader::loadMatrixArray_internal(const std::string& name, const std::vector<MatrixX<T>>& default_value,
                                                                             optional_t optional, unique_t unique)
  {
    const auto resolved_name = m_pp.resolveName(name);
    int rows;
    std::vector<long int> cols;
    bool success = true;
    success = success && m_pp.getParam(resolved_name_t(resolved_name.str + "/rows"), rows);
    success = success && m_pp.getParam(resolved_name_t(resolved_name.str + "/cols"), cols);
 
    std::vector<MatrixX<T>> loaded;
    loaded.reserve(cols.size());
 
    int total_cols = 0;
    /* check correctness of loaded parameters so far calculate the total dimension //{ */
 
    if (!success)
    {
      printError(std::string("Failed to load ") + resolved_name.str + std::string("/rows or ") + resolved_name.str + std::string("/cols"));
      m_load_successful = false;
      return default_value;
    }
    if (rows < 0)
    {
      printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved_name.str + std::string(" (rows and cols must be >= 0)"));
      m_load_successful = false;
      return default_value;
    }
    for (const auto& col : cols)
    {
      if (col < 0)
      {
        printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved_name.str + std::string(" (rows and cols must be >= 0)"));
        m_load_successful = false;
        return default_value;
      }
      total_cols += col;
    }
 
    //}
 
    const auto [loaded_matrix, loaded_ok] = loadMatrixX(name + "/data", MatrixX<T>(), rows, total_cols, optional, unique, NO_SWAP, false);
    /* std::cout << "loaded_matrix: " << loaded_matrix << std::endl; */
    /* std::cout << "loaded_matrix: " << loaded_matrix.rows() << "x" << loaded_matrix.cols() << std::endl; */
    /* std::cout << "expected dims: " << rows << "x" << total_cols << std::endl; */
    if (loaded_matrix.rows() != rows || loaded_matrix.cols() != total_cols)
    {
      m_load_successful = false;
      return default_value;
    }
 
    int cols_loaded = 0;
    for (unsigned it = 0; it < cols.size(); it++)
    {
      const int cur_cols = cols.at(it);
      const MatrixX<T> cur_mat = loaded_matrix.block(0, cols_loaded, rows, cur_cols);
      /* std::cout << "cur_mat: " << cur_mat << std::endl; */
      loaded.push_back(cur_mat);
      cols_loaded += cur_cols;
      printValue(resolved_name_t(resolved_name.str + "/matrix#" + std::to_string(it)), cur_mat);
    }
    return loaded;
  }
  //}
 
  //}
 
  /* load helper function for generic types //{ */
  // This function tries to load a parameter with name 'name' and a default value.
  // You can use the flag 'optional' to not throw a ROS_ERROR when the parameter
  // cannot be loaded and the flag 'printValues' to set whether the loaded
  // value and name of the parameter should be printed to cout.
  // If 'optional' is set to false and the parameter could not be loaded,
  // the flag 'load_successful' is set to false and a ROS_ERROR message
  // is printer.
  // If 'unique' flag is set to false then the parameter is not checked
  // for being loaded twice.
  // Returns a tuple, containing either the loaded or the default value and a bool,
  // indicating if the value was loaded (true) or the default value was used (false).
  template <typename T>
  std::pair<T, bool> ParamLoader::load(const std::string& name, const T& default_value, optional_t optional, unique_t unique)
  {
    const auto resolved_name = m_pp.resolveName(name);
    T loaded = default_value;
    if (unique && check_duplicit_loading(resolved_name))
      return {loaded, false};
 
    bool cur_load_successful = true;
    // try to load the parameter
    const bool success = m_pp.getParam(resolved_name, loaded);
    if (!success)
    {
      // if it was not loaded, set the default value
      loaded = default_value;
      if (!optional)
      {
        // if the parameter was compulsory, alert the user and set the flag
        printError(std::string("Could not load non-optional parameter ") + resolved_name.str);
        cur_load_successful = false;
      }
    }
 
    if (cur_load_successful)
    {
      // everything is fine and just print the resolved_name and value if required
      if (m_print_values)
        printValue(resolved_name, loaded);
      // mark the param resolved_name as successfully loaded
      m_loaded_params.insert(resolved_name);
    } else
    {
      m_load_successful = false;
    }
    // finally, return the resulting value
    return {loaded, success};
  }
  //}
  //}
 
  /* loadParam function for optional parameters //{ */
  template <typename T>
  bool ParamLoader::loadParam(const std::string& name, T& out_value, const T& default_value)
  {
    const auto [ret, success] = load<T>(name, default_value, OPTIONAL, UNIQUE);
    out_value = ret;
    return success;
  }
  template <typename T>
  T ParamLoader::loadParam2(const std::string& name, const T& default_value)
  {
    const auto loaded = load<T>(name, default_value, OPTIONAL, UNIQUE);
    return loaded.first;
  }
  template <typename T>
  bool ParamLoader::loadParamReusable(const std::string& name, T& out_value, const T& default_value)
  {
    const auto [ret, success] = load<T>(name, default_value, OPTIONAL, REUSABLE);
    out_value = ret;
    return success;
  }
  template <typename T>
  T ParamLoader::loadParamReusable2(const std::string& name, const T& default_value)
  {
    const auto [ret, success] = load<T>(name, default_value, OPTIONAL, REUSABLE);
    return ret;
  }
  //}
 
  /* loadParam function for compulsory parameters //{ */
  template <typename T>
  bool ParamLoader::loadParam(const std::string& name, T& out_value)
  {
    const auto [ret, success] = load<T>(name, T(), COMPULSORY, UNIQUE);
    out_value = ret;
    return success;
  }
  template <typename T>
  T ParamLoader::loadParam2(const std::string& name)
  {
    const auto [ret, success] = load<T>(name, T(), COMPULSORY, UNIQUE);
    return ret;
  }
  template <typename T>
  bool ParamLoader::loadParamReusable(const std::string& name, T& out_value)
  {
    const auto [ret, success] = load<T>(name, T(), COMPULSORY, REUSABLE);
    out_value = ret;
    return success;
  }
  template <typename T>
  T ParamLoader::loadParamReusable2(const std::string& name)
  {
    const auto [ret, success] = load<T>(name, T(), COMPULSORY, REUSABLE);
    return ret;
  }
  //}
 
  /* loadParam specializations and convenience functions for Eigen dynamic matrix type //{ */
 
  template <typename T>
  bool ParamLoader::loadParam(const std::string& name, MatrixX<T>& mat, const MatrixX<T>& default_value)
  {
    const int rows = default_value.rows();
    const int cols = default_value.cols();
    const bool loaded_ok = loadMatrixDynamic(name, mat, default_value, rows, cols);
    return loaded_ok;
  }
 
  template <typename T>
  ParamLoader::MatrixX<T> ParamLoader::loadParam2(const std::string& name, const MatrixX<T>& default_value)
  {
    MatrixX<T> ret;
    loadParam(name, ret, default_value);
    return ret;
  }
 
  //}
 
  // loadMatrixStatic function for loading of static Eigen::Matrices //{
 
  template <int rows, int cols, typename T>
  bool ParamLoader::loadMatrixStatic(const std::string& name, Eigen::Matrix<T, rows, cols>& mat)
  {
    const auto [ret, loaded_ok] = loadMatrixStatic_internal<rows, cols, T>(name, Eigen::Matrix<T, rows, cols>::Zero(), COMPULSORY, UNIQUE);
    mat = ret;
    return loaded_ok;
  }
 
  template <int rows, int cols, typename T, typename Derived>
  bool ParamLoader::loadMatrixStatic(const std::string& name, Eigen::Matrix<T, rows, cols>& mat, const Eigen::MatrixBase<Derived>& default_value)
  {
    const auto [ret, loaded_ok] = loadMatrixStatic_internal<rows, cols, T>(name, Eigen::Matrix<T, rows, cols>(default_value), OPTIONAL, UNIQUE);
    mat = ret;
    return loaded_ok;
  }
 
  template <int rows, int cols, typename T>
  Eigen::Matrix<T, rows, cols> ParamLoader::loadMatrixStatic2(const std::string& name)
  {
    Eigen::Matrix<T, rows, cols> ret;
    loadMatrixStatic(name, ret);
    return ret;
  }
 
  template <int rows, int cols, typename T, typename Derived>
  Eigen::Matrix<T, rows, cols> ParamLoader::loadMatrixStatic2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value)
  {
    Eigen::Matrix<T, rows, cols> ret;
    loadMatrixStatic(name, ret, default_value);
    return ret;
  }
  //}
 
  // loadMatrixKnown function for loading of Eigen matrices with known dimensions //{
 
  template <typename T>
  bool ParamLoader::loadMatrixKnown(const std::string& name, MatrixX<T>& mat, int rows, int cols)
  {
    const auto [ret, loaded_ok] = loadMatrixKnown_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
    mat = ret;
    return loaded_ok;
  }
 
  template <typename T, typename Derived>
  bool ParamLoader::loadMatrixKnown(const std::string& name, MatrixX<T>& mat, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
  {
    const auto [ret, loaded_ok] = loadMatrixKnown_internal(name, MatrixX<T>(default_value), rows, cols, OPTIONAL, UNIQUE);
    mat = ret;
    return loaded_ok;
  }
 
  template <typename T>
  ParamLoader::MatrixX<T> ParamLoader::loadMatrixKnown2(const std::string& name, int rows, int cols)
  {
    MatrixX<T> ret;
    loadMatrixKnown(name, ret, rows, cols);
    return ret;
  }
 
  template <typename T, typename Derived>
  ParamLoader::MatrixX<T> ParamLoader::loadMatrixKnown2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
  {
    MatrixX<T> ret;
    loadMatrixKnown(name, ret, default_value, rows, cols);
    return ret;
  }
  //}
 
  // loadMatrixDynamic function for half-dynamic loading of MatrixX<T> //{
 
  template <typename T>
  bool ParamLoader::loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, int rows, int cols)
  {
    const auto [ret, loaded_ok] = loadMatrixDynamic_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
    mat = ret;
    return loaded_ok;
  }
 
  template <typename T, typename Derived>
  bool ParamLoader::loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
  {
    const auto [ret, loaded_ok] = loadMatrixDynamic_internal(name, MatrixX<T>(default_value), rows, cols, OPTIONAL, UNIQUE);
    mat = ret;
    return loaded_ok;
  }
 
  template <typename T>
  ParamLoader::MatrixX<T> ParamLoader::loadMatrixDynamic2(const std::string& name, int rows, int cols)
  {
    MatrixX<T> ret;
    loadMatrixDynamic(name, ret, rows, cols);
    return ret;
  }
 
  template <typename T, typename Derived>
  ParamLoader::MatrixX<T> ParamLoader::loadMatrixDynamic2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
  {
    MatrixX<T> ret;
    loadMatrixDynamic(name, ret, default_value, rows, cols);
    return ret;
  }
 
  //}
 
  // loadMatrixArray function for loading of an array of MatrixX<T> with known dimensions //{
  template <typename T>
  void ParamLoader::loadMatrixArray(const std::string& name, std::vector<MatrixX<T>>& mat)
  {
    mat = loadMatrixArray2<double>(name);
  }
 
  template <typename T>
  void ParamLoader::loadMatrixArray(const std::string& name, std::vector<MatrixX<T>>& mat, const std::vector<MatrixX<T>>& default_value)
  {
    mat = loadMatrixArray2(name, default_value);
  }
 
  template <typename T>
  std::vector<ParamLoader::MatrixX<T>> ParamLoader::loadMatrixArray2(const std::string& name)
  {
    return loadMatrixArray_internal(name, std::vector<MatrixX<T>>(), COMPULSORY, UNIQUE);
  }
 
  template <typename T>
  std::vector<ParamLoader::MatrixX<T>> ParamLoader::loadMatrixArray2(const std::string& name, const std::vector<MatrixX<T>>& default_value)
  {
    return loadMatrixArray_internal(name, default_value, OPTIONAL, UNIQUE);
  }
  //}
 
} // namespace mrs_lib