voronoi_graph_node.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <tuw_voronoi_map/voronoi_path_generator.h>
#include <tuw_voronoi_graph/voronoi_graph_node.h>
#include <tuw_voronoi_graph/voronoi_graph_generator.h>
#include <memory>
#include <tuw_multi_robot_msgs/Graph.h>
#include <string>
 
void publishTf(std::string mapName);
 
 
int main(int argc, char **argv)
{
 
    ros::init(argc, argv, "voronoi_graph_node"); 
    ros::NodeHandle n;
 
    tuw_graph::VoronoiGeneratorNode mapNode(n);
 
    return 0;
}
 
namespace tuw_graph
{
    VoronoiGeneratorNode::VoronoiGeneratorNode(ros::NodeHandle &n) : voronoi_map::VoronoiPathGenerator(), VoronoiGraphGenerator(), Serializer(), n_(n), n_param_("~")
    {
 
 
        double loop_rate;
        n_param_.param<double>("loop_rate", loop_rate, 0.1);
 
        n_param_.param<bool>("publish_voronoi_map_image", publishVoronoiMapImage_, false);
 
        n_param_.param<double>("map_inflation", inflation_, 0.1); 
 
        n_param_.param<float>("segment_length", segment_length_, 1.0); 
 
 
        crossingOptimization_ = 0.2;
        n_param_.param<float>("opt_crossings", crossingOptimization_, 0.2);
 
        n_param_.param<float>("opt_end_segments", endSegmentOptimization_, 0.2);
        //endSegmentOptimization_ = std::min<float>(endSegmentOptimization_, 0.7 * path_length_);
 
        n_param_.param<std::string>("graph_cache_path", graphCachePath_, "/tmp");
 
        if (graphCachePath_.back() != '/'){
            graphCachePath_ += "/";
        }
        n_param_.param<std::string>("custom_graph_path", customGraphPath_, "");
 
        if (customGraphPath_.back() != '/' && customGraphPath_.size() != 0){
            customGraphPath_ += "/";
        }
 
        subMap_ = n.subscribe("map", 1, &VoronoiGeneratorNode::globalMapCallback, this);
        if(publishVoronoiMapImage_){
            pubVoronoiMapImage_    = n.advertise<nav_msgs::OccupancyGrid>( "map_eroded", 1);
        }
        pubSegments_ = n.advertise<tuw_multi_robot_msgs::Graph>("segments", 1);
 
 
        ros::Rate r(loop_rate);
 
        ROS_INFO("Initialization done!");
 
        while (ros::ok())
        {
            ros::spinOnce();
 
            publishSegments();
 
            r.sleep();
        }
 
    }
 
    void VoronoiGeneratorNode::globalMapCallback(const nav_msgs::OccupancyGrid::ConstPtr &_map)
    {
        std::vector<signed char> map = _map->data;
 
 
        std::vector<double> parameters;
        parameters.push_back(_map->info.origin.position.x);
        parameters.push_back(_map->info.origin.position.y);
        parameters.push_back(_map->info.resolution);
        parameters.push_back(inflation_);
        parameters.push_back(segment_length_);
        parameters.push_back(endSegmentOptimization_);
        parameters.push_back(crossingOptimization_);
 
        size_t new_hash = getHash(map, parameters);
 
 
        if (customGraphPath_.size() == 0)
        {
            if (new_hash != current_map_hash_ )
            {
                if (!loadGraph(new_hash) )
                {
                    ROS_INFO("Graph generator: Graph not found! Generating new one!");
                    createGraph(_map, new_hash);
                }
                else
                {
                    ROS_INFO("Graph generator: Graph loaded from memory");
                }
 
                current_map_hash_ = new_hash;
            }
        }
        else
        {
            ROS_INFO("loading custom graph from: %s", customGraphPath_.c_str());
            if (loadCustomGraph(customGraphPath_))
                ROS_INFO("graph loaded");
            else
                ROS_INFO("failed to load graph");
        }
 
        if (publishVoronoiMapImage_ && (map_.size > 0))
        {
            voronoiMapImage_.header = _map->header;
            voronoiMapImage_.info = _map->info;
            voronoiMapImage_.data.resize(_map->data.size());
 
            for (unsigned int i = 0; i < voronoiMapImage_.data.size(); i++)
            {
                voronoiMapImage_.data[i] = map_.data[i];
            }
            pubVoronoiMapImage_.publish(voronoiMapImage_);
        }
 
    }
 
    void VoronoiGeneratorNode::createGraph(const nav_msgs::OccupancyGrid::ConstPtr &_map, size_t _map_hash)
    {
        std::vector<signed char> map = _map->data;
 
        segments_.clear();
        Segment::resetId();
 
        ROS_INFO("Graph generator: Computing distance field ...");
        origin_[0] = _map->info.origin.position.x;
        origin_[1] = _map->info.origin.position.y;
        resolution_ = _map->info.resolution;
 
        cv::Mat m(_map->info.height, _map->info.width, CV_8SC1, map.data());
        prepareMap(m, map_, inflation_ / _map->info.resolution);
        computeDistanceField(map_, distField_);
 
        ROS_INFO("Graph generator: Computing voronoi graph ...");
        computeVoronoiMap(distField_, voronoiMap_);
 
        ROS_INFO("Graph generator: Generating graph ...");
        potential.reset(new float[m.cols * m.rows]);
        float pixel_path_length = segment_length_ / resolution_;
        segments_ = calcSegments(m, distField_, voronoiMap_, potential.get(), pixel_path_length, crossingOptimization_ / resolution_, endSegmentOptimization_ / resolution_);
 
        //Check Directroy
        save(graphCachePath_ + std::to_string(_map_hash) + "/", segments_, origin_, resolution_, map_);
        ROS_INFO("Graph generator: Created new Graph %lu", _map_hash);
    }
 
    bool VoronoiGeneratorNode::loadGraph(std::size_t _hash)
    {
        segments_.clear();
        Segment::resetId();
        return load(graphCachePath_ + std::to_string(_hash) + "/", segments_, origin_, resolution_, map_);
    }
 
    bool VoronoiGeneratorNode::loadCustomGraph(std::string _path)
    {
        segments_.clear();
        Segment::resetId();
        return load(_path, segments_, origin_, resolution_);
    }
 
    void VoronoiGeneratorNode::publishSegments()
    {
        tuw_multi_robot_msgs::Graph graph;
        graph.header.frame_id = "map";
        graph.header.seq = 0;
        graph.header.stamp = ros::Time::now();
 
        graph.origin.position.x = origin_[0]; //TODO test
        graph.origin.position.y = origin_[1]; //TODO test
 
        for (auto it = segments_.begin(); it != segments_.end(); ++it)
        {
            tuw_multi_robot_msgs::Vertex seg;
 
            seg.id = (*it).getId();
            seg.weight = (*it).getLength();
            seg.width = (*it).getMinPathSpace() * resolution_;
            seg.valid = true;
            std::vector<Eigen::Vector2d> path = (*it).getPath();
 
            for (uint32_t i = 0; i < path.size(); i++)
            {
                geometry_msgs::Point pos;
                pos.x = path[i][0] * resolution_;
                pos.y = path[i][1] * resolution_;
                pos.z = 0;
 
                seg.path.push_back(pos);
            }
 
            //ROS_INFO("distORIG: %i/%i", (*it)->GetPredecessors().size(), (*it)->GetSuccessors().size());
            std::vector<uint32_t> predecessors = (*it).getPredecessors();
 
            for (uint32_t i = 0; i < predecessors.size(); i++)
            {
                seg.predecessors.push_back(predecessors[i]);
            }
 
            std::vector<uint32_t> successors = (*it).getSuccessors();
 
            for (uint32_t i = 0; i < successors.size(); i++)
            {
                seg.successors.push_back(successors[i]);
            }
 
            graph.vertices.push_back(seg);
        }
 
        pubSegments_.publish(graph);
    }
 
} // namespace tuw_graph