GridMapMsgHelpers.hpp

Go to the documentation of this file.

/*
 * GridMapMsgHelpers.hpp
 *
 *  Created on: Sep 8, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#pragma once

// ROS
#include <ros/ros.h>
#include <std_msgs/UInt32MultiArray.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Float64MultiArray.h>

// Eigen
#include <Eigen/Core>

namespace grid_map {

int nDimensions();

enum class StorageIndices {
    Column,
    Row
};

extern std::map<StorageIndices, std::string> storageIndexNames;

template<typename MultiArrayMessageType_>
bool isRowMajor(const MultiArrayMessageType_& message)
{
  if (message.layout.dim[0].label == grid_map::storageIndexNames[grid_map::StorageIndices::Column]) return false;
  else if (message.layout.dim[0].label == grid_map::storageIndexNames[grid_map::StorageIndices::Row]) return true;
  ROS_ERROR("isRowMajor() failed because layout label is not set correctly.");
  return false;
}

template<typename MultiArrayMessageType_>
unsigned int getCols(const MultiArrayMessageType_& message)
{
  if (isRowMajor(message)) return message.layout.dim.at(1).size;
  return message.layout.dim.at(0).size;
}

template<typename MultiArrayMessageType_>
unsigned int getRows(const MultiArrayMessageType_& message)
{
  if (isRowMajor(message)) return message.layout.dim.at(0).size;
  return message.layout.dim.at(1).size;
}

template<typename EigenType_, typename MultiArrayMessageType_>
bool matrixEigenCopyToMultiArrayMessage(const EigenType_& e, MultiArrayMessageType_& m)
{
  m.layout.dim.resize(nDimensions());
  m.layout.dim[0].stride = e.size();
  m.layout.dim[0].size = e.outerSize();
  m.layout.dim[1].stride = e.innerSize();
  m.layout.dim[1].size = e.innerSize();

  if (e.IsRowMajor) {
    m.layout.dim[0].label = storageIndexNames[StorageIndices::Row];
    m.layout.dim[1].label = storageIndexNames[StorageIndices::Column];
  } else {
    m.layout.dim[0].label = storageIndexNames[StorageIndices::Column];
    m.layout.dim[1].label = storageIndexNames[StorageIndices::Row];
  }

  m.data.insert(m.data.begin() + m.layout.data_offset, e.data(), e.data() + e.size());
  return true;
}

template<typename EigenType_, typename MultiArrayMessageType_>
bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_& m, EigenType_& e)
{
  if (e.IsRowMajor != isRowMajor(m)) {
    ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible.");
    return false;
  }

  EigenType_ tempE(getRows(m), getCols(m));
  tempE = Eigen::Map<const EigenType_>(m.data.data(), getRows(m), getCols(m));
  e = tempE;
  return true;
}

template<typename EigenType_, typename MultiArrayMessageType_>
bool multiArrayMessageMapToMatrixEigen(MultiArrayMessageType_& m, EigenType_& e)
{
  if (e.IsRowMajor != isRowMajor(m)) {
    ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible.");
    return false;
  }

  e.resize(getRows(m), getCols(m));
  e = Eigen::Map<EigenType_>(m.data.data(), getRows(m), getCols(m));
  return true;
}

} /* namespace */