GridMapMsgHelpers.hpp
Go to the documentation of this file.
1 /*
2  * GridMapMsgHelpers.hpp
3  *
4  * Created on: Sep 8, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
9 #pragma once
10 
11 // ROS
12 #include <ros/ros.h>
13 #include <std_msgs/UInt32MultiArray.h>
14 #include <std_msgs/Float32MultiArray.h>
15 #include <std_msgs/Float64MultiArray.h>
16 
17 // Eigen
18 #include <Eigen/Core>
19 
20 namespace grid_map {
21 
26 int nDimensions();
27 
28 enum class StorageIndices {
29  Column,
30  Row
31 };
32 
34 extern std::map<StorageIndices, std::string> storageIndexNames;
35 
42 template<typename MultiArrayMessageType_>
43 bool isRowMajor(const MultiArrayMessageType_& message)
44 {
45  if (message.layout.dim[0].label == grid_map::storageIndexNames[grid_map::StorageIndices::Column]) return false;
46  else if (message.layout.dim[0].label == grid_map::storageIndexNames[grid_map::StorageIndices::Row]) return true;
47  ROS_ERROR("isRowMajor() failed because layout label is not set correctly.");
48  return false;
49 }
50 
57 template<typename MultiArrayMessageType_>
58 unsigned int getCols(const MultiArrayMessageType_& message)
59 {
60  if (isRowMajor(message)) return message.layout.dim.at(1).size;
61  return message.layout.dim.at(0).size;
62 }
63 
70 template<typename MultiArrayMessageType_>
71 unsigned int getRows(const MultiArrayMessageType_& message)
72 {
73  if (isRowMajor(message)) return message.layout.dim.at(0).size;
74  return message.layout.dim.at(1).size;
75 }
76 
87 template<typename EigenType_, typename MultiArrayMessageType_>
88 bool matrixEigenCopyToMultiArrayMessage(const EigenType_& e, MultiArrayMessageType_& m)
89 {
90  m.layout.dim.resize(nDimensions());
91  m.layout.dim[0].stride = e.size();
92  m.layout.dim[0].size = e.outerSize();
93  m.layout.dim[1].stride = e.innerSize();
94  m.layout.dim[1].size = e.innerSize();
95 
96  if (e.IsRowMajor) {
97  m.layout.dim[0].label = storageIndexNames[StorageIndices::Row];
98  m.layout.dim[1].label = storageIndexNames[StorageIndices::Column];
99  } else {
100  m.layout.dim[0].label = storageIndexNames[StorageIndices::Column];
101  m.layout.dim[1].label = storageIndexNames[StorageIndices::Row];
102  }
103 
104  m.data.insert(m.data.begin() + m.layout.data_offset, e.data(), e.data() + e.size());
105  return true;
106 }
107 
116 template<typename EigenType_, typename MultiArrayMessageType_>
117 bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_& m, EigenType_& e)
118 {
119  if (e.IsRowMajor != isRowMajor(m)) {
120  ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible.");
121  return false;
122  }
123 
124  EigenType_ tempE(getRows(m), getCols(m));
125  tempE = Eigen::Map<const EigenType_>(m.data.data(), getRows(m), getCols(m));
126  e = tempE;
127  return true;
128 }
129 
137 template<typename EigenType_, typename MultiArrayMessageType_>
138 bool multiArrayMessageMapToMatrixEigen(MultiArrayMessageType_& m, EigenType_& e)
139 {
140  if (e.IsRowMajor != isRowMajor(m)) {
141  ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible.");
142  return false;
143  }
144 
145  e.resize(getRows(m), getCols(m));
146  e = Eigen::Map<EigenType_>(m.data.data(), getRows(m), getCols(m));
147  return true;
148 }
149 
150 } /* namespace */
bool multiArrayMessageMapToMatrixEigen(MultiArrayMessageType_ &m, EigenType_ &e)
unsigned int getRows(const MultiArrayMessageType_ &message)
bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_ &m, EigenType_ &e)
bool matrixEigenCopyToMultiArrayMessage(const EigenType_ &e, MultiArrayMessageType_ &m)
std::map< StorageIndices, std::string > storageIndexNames
Holds the names of the storage indeces.
unsigned int getCols(const MultiArrayMessageType_ &message)
bool isRowMajor(const MultiArrayMessageType_ &message)
#define ROS_ERROR(...)


grid_map_ros
Author(s): Péter Fankhauser
autogenerated on Wed Jul 5 2023 02:23:44