.. _program_listing_file__tmp_ws_src_grid_map_grid_map_ros_include_grid_map_ros_GridMapMsgHelpers.hpp:

Program Listing for File GridMapMsgHelpers.hpp
==============================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_grid_map_grid_map_ros_include_grid_map_ros_GridMapMsgHelpers.hpp>` (``/tmp/ws/src/grid_map/grid_map_ros/include/grid_map_ros/GridMapMsgHelpers.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * 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_