GridMapMsgHelpers.hpp
Go to the documentation of this file.
00001 /*
00002  * GridMapMsgHelpers.hpp
00003  *
00004  *  Created on: Sep 8, 2014
00005  *      Author: Péter Fankhauser
00006  *   Institute: ETH Zurich, Autonomous Systems Lab
00007  */
00008 
00009 #pragma once
00010 
00011 // ROS
00012 #include <ros/ros.h>
00013 #include <std_msgs/UInt32MultiArray.h>
00014 #include <std_msgs/Float32MultiArray.h>
00015 #include <std_msgs/Float64MultiArray.h>
00016 
00017 // Eigen
00018 #include <Eigen/Core>
00019 
00020 namespace grid_map {
00021 
00026 const int nDimensions();
00027 
00028 enum class StorageIndices {
00029     Column,
00030     Row
00031 };
00032 
00034 extern std::map<StorageIndices, std::string> storageIndexNames;
00035 
00042 template<typename MultiArrayMessageType_>
00043 bool isRowMajor(const MultiArrayMessageType_& message)
00044 {
00045   if (message.layout.dim[0].label == grid_map::storageIndexNames[grid_map::StorageIndices::Column]) return false;
00046   else if (message.layout.dim[0].label == grid_map::storageIndexNames[grid_map::StorageIndices::Row]) return true;
00047   ROS_ERROR("isRowMajor() failed because layout label is not set correctly.");
00048   return false;
00049 }
00050 
00057 template<typename MultiArrayMessageType_>
00058 unsigned int getCols(const MultiArrayMessageType_& message)
00059 {
00060   if (isRowMajor(message)) return message.layout.dim.at(1).size;
00061   return message.layout.dim.at(0).size;
00062 }
00063 
00070 template<typename MultiArrayMessageType_>
00071 unsigned int getRows(const MultiArrayMessageType_& message)
00072 {
00073   if (isRowMajor(message)) return message.layout.dim.at(0).size;
00074   return message.layout.dim.at(1).size;
00075 }
00076 
00087 template<typename EigenType_, typename MultiArrayMessageType_>
00088 bool matrixEigenCopyToMultiArrayMessage(const EigenType_& e, MultiArrayMessageType_& m)
00089 {
00090   m.layout.dim.resize(nDimensions());
00091   m.layout.dim[0].stride = e.size();
00092   m.layout.dim[0].size = e.outerSize();
00093   m.layout.dim[1].stride = e.innerSize();
00094   m.layout.dim[1].size = e.innerSize();
00095 
00096   if (e.IsRowMajor) {
00097     m.layout.dim[0].label = storageIndexNames[StorageIndices::Row];
00098     m.layout.dim[1].label = storageIndexNames[StorageIndices::Column];
00099   } else {
00100     m.layout.dim[0].label = storageIndexNames[StorageIndices::Column];
00101     m.layout.dim[1].label = storageIndexNames[StorageIndices::Row];
00102   }
00103 
00104   m.data.insert(m.data.begin() + m.layout.data_offset, e.data(), e.data() + e.size());
00105   return true;
00106 }
00107 
00116 template<typename EigenType_, typename MultiArrayMessageType_>
00117 bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_& m, EigenType_& e)
00118 {
00119   if (e.IsRowMajor != isRowMajor(m)) {
00120     ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible.");
00121     return false;
00122   }
00123 
00124   EigenType_ tempE(getRows(m), getCols(m));
00125   tempE = Eigen::Map<const EigenType_>(m.data.data(), getRows(m), getCols(m));
00126   e = tempE;
00127   return true;
00128 }
00129 
00137 template<typename EigenType_, typename MultiArrayMessageType_>
00138 bool multiArrayMessageMapToMatrixEigen(MultiArrayMessageType_& m, EigenType_& e)
00139 {
00140   if (e.IsRowMajor != isRowMajor(m)) {
00141     ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible.");
00142     return false;
00143   }
00144 
00145   e.resize(getRows(m), getCols(m));
00146   e = Eigen::Map<EigenType_>(m.data.data(), getRows(m), getCols(m));
00147   return true;
00148 }
00149 
00150 } /* namespace */


grid_map_ros
Author(s): Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:28