Program Listing for File GridMapMsgHelpers.hpp
↰ Return to documentation for file (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_