voronoi_graph_node.h

Go to the documentation of this file.

#ifndef VORONOI_GRAPH_NODE_H
#define VORONOI_GRAPH_NODE_H

#include <memory>

// ROS
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>

#include <tuw_voronoi_map/voronoi_path_generator.h>
#include <tuw_voronoi_graph/voronoi_graph_generator.h>

#include <tuw_serialization/serializer.h>

namespace tuw_graph
{


    class VoronoiGeneratorNode : public voronoi_map::VoronoiPathGenerator, public VoronoiGraphGenerator, public Serializer
    {

        public:
            VoronoiGeneratorNode(ros::NodeHandle & n);

            ros::NodeHandle n_;
            ros::NodeHandle n_param_;
            std::unique_ptr<ros::Rate> rate_;
            void publishMap();
            void publishSegments();

        private:
            void globalMapCallback(const nav_msgs::OccupancyGrid::ConstPtr& _map);
            void createGraph(const nav_msgs::OccupancyGrid::ConstPtr& _map, size_t _map_hash);
            bool loadGraph(size_t _hash);
            bool loadCustomGraph(std::string _path);
            
            
            ros::Publisher                          pubVoronoiMap_;
            ros::Publisher                          pubSegments_;
            ros::Subscriber                         subMap_;
  

            std::string                             topicGlobalMap_;
            std::string                             topicVoronoiMap_;
            std::string                             topicSegments_;
            std::string                             graphCachePath_;
            std::string                             customGraphPath_;
    
            std::string                             frameGlobalMap_;
            std::string                             frameVoronoiMap_;


            size_t                                  current_map_hash_;
            Eigen::Vector2d                         origin_;
            float                                   resolution_;
            cv::Mat                                 map_;
            cv::Mat                                 distField_;
            cv::Mat                                 voronoiMap_;
            float                                   path_length_;
            std::unique_ptr<float[]>                potential;
            std::vector<Segment>                    segments_;
            int                                     smoothing_;
            float                                   crossingOptimization_;
            float                                   endSegmentOptimization_;
    };

}

#endif // TUW_NAV_COSTMAP_NODE_H