terrain_map_publisher.h

#ifndef TERRAIN_MAP_PUBLISHER_H
#define TERRAIN_MAP_PUBLISHER_H
 
#include <ros/package.h>
#include <ros/ros.h>
 
#include <fstream>  // ifstream
#include <grid_map_core/grid_map_core.hpp>
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <grid_map_ros/grid_map_ros.hpp>
#include <iostream>  // cout
#include <sstream>   // istringstream
#include <string>
#include <vector>
 
struct Obstacle {
  double x;
  double y;
  double height;
  double radius;
};
 
struct Step {
  double x;
  double height;
};
 
 
class TerrainMapPublisher {
 public:
  TerrainMapPublisher(ros::NodeHandle nh);
 
  void updateParams();
 
  void createMap();
 
  void updateMap();
 
  std::vector<std::vector<double> > loadCSV(std::string filename);
 
  void loadMapFromCSV();
 
  void loadMapFromImage(const sensor_msgs::Image& msg);
 
  void publishMap();
 
  void spin();
 
 private:
  ros::Subscriber image_sub_;
 
  ros::Publisher terrain_map_pub_;
 
  ros::NodeHandle nh_;
 
  double update_rate_;
 
  std::string map_frame_;
 
  grid_map::GridMap terrain_map_;
 
  std::string terrain_type_;
 
  std::string map_data_source_;
 
  bool map_initialized_ = false;
 
  double resolution_;
 
  double min_height_;
 
  double max_height_;
 
  Obstacle obstacle_;
 
  Step step1_;
 
  Step step2_;
};
 
#endif  // TERRAIN_MAP_PUBLISHER_H