mst_path.h

Go to the documentation of this file.

#ifndef MST_PATH_H
#define MST_PATH_H

#include <deque>
#include <valarray>
#include <unordered_set>
#include <ros/ros.h>
#include <tf2/utils.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseArray.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include "lib/edge.h"

using namespace std;
using namespace ros;

class mst_path
{
public:
    mst_path ();

    void generate_path (geometry_msgs::Point);

    geometry_msgs::PoseArray get_grid ();

    nav_msgs::Path get_path ();

    geometry_msgs::Point get_waypoint (geometry_msgs::Point position, double tolerance);

    void initialize_graph (nav_msgs::OccupancyGrid gridmap, bool connect4 = true);

    void initialize_tree (vector<edge> mst);

    bool valid ();

private:
    void add_edge(int from, int to, int cost);

    double dist (geometry_msgs::Point p1, geometry_msgs::Point p2);

    geometry_msgs::Point get_wp (int offset=0);

    void remove_edge (edge e);

    deque<geometry_msgs::Point> path;

    unordered_set<edge, hash_edge> edges;

    vector<unordered_set<int>> nodes;

    nav_msgs::OccupancyGrid map;

    int wp;
};

#endif // MST_PATH_H