converter.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef cost_map_ros_CONVERTER_HPP_
#define cost_map_ros_CONVERTER_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

// grid maps
#include <grid_map_core/grid_map_core.hpp>
#include <grid_map_ros/grid_map_ros.hpp>
#include <grid_map_costmap_2d/grid_map_costmap_2d.hpp>
#include <cost_map_core/cost_map_core.hpp>
#include <cost_map_msgs/CostMap.h>
#include <cost_map_msgs/GetCostMap.h>

// ros
#include <costmap_2d/costmap_2d_ros.h>
#include <nav_msgs/OccupancyGrid.h>

// general
#include <string>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace cost_map {

/*****************************************************************************
** CostMap and GridMap
*****************************************************************************/

void toGridMap(const cost_map::CostMap cost_map, grid_map::GridMap& grid_map);

/*****************************************************************************
** ROS Messages
*****************************************************************************/

void toMessage(const cost_map::CostMap& cost_map, cost_map_msgs::CostMap& message);

bool fromMessage(const cost_map_msgs::CostMap& message, cost_map::CostMap& cost_map);

/*****************************************************************************
** ROS Images
*****************************************************************************/

bool addLayerFromROSImage(const sensor_msgs::Image& image,
                          const std::string& layer_name,
                          cost_map::CostMap& cost_map
                          );

/*****************************************************************************
** Ros CostMap2D & Occupancy Grids
*****************************************************************************/
/*
 * There are various ways we can pull from ros costmaps. Build up a suite
 * of functions as we need to.
 */

bool fromCostmap2DROS(costmap_2d::Costmap2DROS& ros_costmap,
                      const std::string& layer_name,
                      cost_map::CostMap& cost_map);

bool fromCostmap2DROSAtRobotPose(costmap_2d::Costmap2DROS& ros_costmap,
                                 const cost_map::Length& geometry,
                                 const std::string& layer_name,
                                 cost_map::CostMap& cost_map);

void toOccupancyGrid(const cost_map::CostMap& cost_map, const std::string& layer, nav_msgs::OccupancyGrid& msg);

class Costmap2DROSServiceProvider {
public:
  Costmap2DROSServiceProvider(costmap_2d::Costmap2DROS* ros_costmap,
                              const std::string& service_name="get_cost_map");
  Costmap2DROSServiceProvider(costmap_2d::Costmap2DROS* ros_costmap, ros::NodeHandle& node_handle,
                              const std::string& service_name="get_cost_map");

  bool callback(cost_map_msgs::GetCostMap::Request  &req,
                cost_map_msgs::GetCostMap::Response &res);
private:
  costmap_2d::Costmap2DROS* ros_costmap;
  ros::ServiceServer service;
};

/*****************************************************************************
** Trailers
*****************************************************************************/

} // namespace cost_map

#endif /* cost_map_ros_CONVERTER_HPP_ */