GridMapRosConverter.hpp

Go to the documentation of this file.

/*
 * GridMapRosConverter.hpp
 *
 *  Created on: Jul 14, 2014
 *      Author: Péter Fankhauser
 *       Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include <grid_map_core/grid_map_core.hpp>
#include <grid_map_sdf/SignedDistanceField.hpp>
#include <grid_map_msgs/GridMap.h>

// STL
#include <vector>
#include <unordered_map>

// Eigen
#include <Eigen/Core>

// ROS
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GridCells.h>
#include <cv_bridge/cv_bridge.h>

namespace grid_map {

class GridMapRosConverter
{
 public:
  GridMapRosConverter();

  virtual ~GridMapRosConverter();

  static bool fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap, const std::vector<std::string>& layers, bool copyBasicLayers = true, bool copyAllNonBasicLayers = true);

  static bool fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap);

  static void toMessage(const grid_map::GridMap& gridMap, grid_map_msgs::GridMap& message);

  static void toMessage(const grid_map::GridMap& gridMap, const std::vector<std::string>& layers,
                        grid_map_msgs::GridMap& message);

  static void toPointCloud(const grid_map::GridMap& gridMap,
                           const std::string& pointLayer,
                           sensor_msgs::PointCloud2& pointCloud);

  static void toPointCloud(const grid_map::GridMap& gridMap,
                           const std::vector<std::string>& layers,
                           const std::string& pointLayer,
                           sensor_msgs::PointCloud2& pointCloud);

  static void toPointCloud(
      const grid_map::SignedDistanceField& signedDistanceField, sensor_msgs::PointCloud2& pointCloud, size_t decimation = 1,
      const std::function<bool(float)>& condition = [](float) { return true; });

  static bool fromOccupancyGrid(const nav_msgs::OccupancyGrid& occupancyGrid,
                                const std::string& layer, grid_map::GridMap& gridMap);

  static void toOccupancyGrid(const grid_map::GridMap& gridMap, const std::string& layer,
                              float dataMin, float dataMax, nav_msgs::OccupancyGrid& occupancyGrid);

  static void toGridCells(const grid_map::GridMap& gridMap, const std::string& layer,
                          float lowerThreshold, float upperThreshold,
                          nav_msgs::GridCells& gridCells);

  static bool initializeFromImage(const sensor_msgs::Image& image, const double resolution,
                                  grid_map::GridMap& gridMap,
                                  const grid_map::Position& position = grid_map::Position::Zero());

  static bool addLayerFromImage(const sensor_msgs::Image& image, const std::string& layer,
                                grid_map::GridMap& gridMap, const float lowerValue = 0.0,
                                const float upperValue = 1.0, const double alphaThreshold = 0.5);

  static bool addColorLayerFromImage(const sensor_msgs::Image& image, const std::string& layer,
                                     grid_map::GridMap& gridMap);

  static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
                      const std::string encoding, sensor_msgs::Image& image);

  static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
                      const std::string encoding, const float lowerValue, const float upperValue,
                      sensor_msgs::Image& image);

  static bool toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
                        const std::string encoding, cv_bridge::CvImage& cvImage);

  static bool toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
                        const std::string encoding, const float lowerValue,
                        const float upperValue, cv_bridge::CvImage& cvImage);

  static bool saveToBag(const grid_map::GridMap& gridMap, const std::string& pathToBag,
                        const std::string& topic);

  static bool loadFromBag(const std::string& pathToBag, const std::string& topic,
                          grid_map::GridMap& gridMap);

};

} /* namespace */