static_map.hpp

Go to the documentation of this file.

#ifndef mitre_fast_layered_map_static_map
#define mitre_fast_layered_map_static_map
#pragma once

// STD
#include <string>

// ROS
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <visualization_msgs/Marker.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

// Grid map
#include <grid_map_core/grid_map_core.hpp>
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <grid_map_ros/grid_map_ros.hpp>
#include <grid_map_msgs/GridMap.h>

namespace mitre_fast_layered_map
{
    struct StaticMapConfiguration
    {
        std::string mapName; 
        std::string staticMapSubTopic; 
        std::string markerSubTopic; 
        std::string gridMapSubTopic; 
        std::string gridMapLayer; 

        std::string occupancyOutputTopic; 
        std::string gridmapOutputTopic; 
    };

    class StaticMap
    {
        public:
        StaticMap();
        StaticMap(ros::NodeHandle *, StaticMapConfiguration);

        ~StaticMap();

        int init();
        int once();

        void staticMapCb(const nav_msgs::OccupancyGrid &);
        void mapMarkerCb(const visualization_msgs::Marker &);
        void gridMapCb(const grid_map_msgs::GridMap &);

        int publishMap();

        private:

        // ROS ECOSYSTEM
        ros::NodeHandle nh_;
        ros::Subscriber staticMapSub_;
        ros::Subscriber markerSub_;
        ros::Subscriber gridMapSub_;
        ros::Publisher occGridPub_; 
        ros::Publisher gridMapPub_;

        // TRANSFORMS
        tf2_ros::Buffer tfBuffer_;              
        tf2_ros::TransformListener tfListener_; 

        // STATE DATA
        bool initialized_{false};
        bool recStaticMap_{false};
        grid_map::GridMap gridMap_;

        StaticMapConfiguration config_;

    };
}

#endif