Program Listing for File GridMapMsgHelpers.hpp

↰ Return to documentation for file (/tmp/ws/src/grid_map/grid_map_ros/include/grid_map_ros/GridMapMsgHelpers.hpp)

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

#ifndef GRID_MAP_ROS__GRIDMAPMSGHELPERS_HPP_
#define GRID_MAP_ROS__GRIDMAPMSGHELPERS_HPP_

// ROS
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/u_int32_multi_array.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>

// Eigen
#include <Eigen/Core>

#include <map>
#include <string>

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;}
  RCLCPP_ERROR(
    rclcpp::get_logger(
      "isRowMajor"), "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)) {
    RCLCPP_ERROR(
      rclcpp::get_logger("multiArrayMessageCopyToMatrixEigen"), "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)) {
    RCLCPP_ERROR(
      rclcpp::get_logger("multiArrayMessageMapToMatrixEigen"), "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 grid_map
#endif  // GRID_MAP_ROS__GRIDMAPMSGHELPERS_HPP_