Public Member Functions | Private Member Functions | Private Attributes | List of all members
mst_path Class Reference

An class to compute the coverage path based on a minimum-spanning-tree (MST). More...

#include <mst_path.h>

Public Member Functions

void generate_path (geometry_msgs::Point)
 Generate all way points for the path. More...
 
geometry_msgs::PoseArray get_grid ()
 Get the graph generated from the grid map. More...
 
nav_msgs::Path get_path ()
 Get the complete path. More...
 
geometry_msgs::Point get_waypoint (geometry_msgs::Point position, double tolerance)
 Get the current waypoint and possibly select next waypoint, if close enough. More...
 
void initialize_graph (nav_msgs::OccupancyGrid gridmap, bool connect4=true)
 Initialize the internal graph structure that represents the area division. More...
 
void initialize_tree (vector< edge > mst)
 Remove edges of the graph that overlap with the tree. More...
 
 mst_path ()
 Constructor. More...
 
bool valid ()
 Check whether the current waypoint index is valid. More...
 

Private Member Functions

void add_edge (int from, int to, int cost)
 Add an edge to the tree graph. More...
 
double dist (geometry_msgs::Point p1, geometry_msgs::Point p2)
 Calculate the distance between two points. More...
 
geometry_msgs::Point get_wp (int offset=0)
 Get the current way point. More...
 
void remove_edge (edge e)
 Remove an edge from the tree graph. More...
 

Private Attributes

unordered_set< edge, hash_edgeedges
 Priority queue of edge objects sorted by cost. More...
 
nav_msgs::OccupancyGrid map
 The grid map that needs to be covered by the MST path. More...
 
vector< unordered_set< int > > nodes
 The sets of connected vertices. More...
 
deque< geometry_msgs::Point > path
 The boustrophedon path. More...
 
int wp
 The current way point. More...
 

Detailed Description

An class to compute the coverage path based on a minimum-spanning-tree (MST).

Definition at line 21 of file mst_path.h.

Constructor & Destructor Documentation

◆ mst_path()

mst_path::mst_path ( )

Constructor.

Definition at line 3 of file mst_path.cpp.

Member Function Documentation

◆ add_edge()

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

Add an edge to the tree graph.

Parameters
fromThe starting vertex.
toThe ending vertex.
costThe cost of the edge.

Definition at line 240 of file mst_path.cpp.

◆ dist()

double mst_path::dist ( geometry_msgs::Point  p1,
geometry_msgs::Point  p2 
)
private

Calculate the distance between two points.

Parameters
p1The first point.
p2The second point.
Returns
The straight line distance between both points.

Definition at line 251 of file mst_path.cpp.

◆ generate_path()

void mst_path::generate_path ( geometry_msgs::Point  start)

Generate all way points for the path.

Parameters
startThe current position of the CPS.

Definition at line 7 of file mst_path.cpp.

◆ get_grid()

geometry_msgs::PoseArray mst_path::get_grid ( )

Get the graph generated from the grid map.

Returns
An array of poses representing the vertices of the grid.

Definition at line 97 of file mst_path.cpp.

◆ get_path()

nav_msgs::Path mst_path::get_path ( )

Get the complete path.

Returns
The path as vector of poses.

Definition at line 126 of file mst_path.cpp.

◆ get_waypoint()

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

Get the current waypoint and possibly select next waypoint, if close enough.

Parameters
positionThe current position of the CPS.
toleranceThe distance to the current waypoint below which the next waypoint is selected.
Returns
A waypoint for the CPS to navigate to.

Definition at line 141 of file mst_path.cpp.

◆ get_wp()

geometry_msgs::Point mst_path::get_wp ( int  offset = 0)
private

Get the current way point.

Parameters
offsetThe index offset to the current waypoint.
Returns
The way point, if a valid offset was given. An empty point otherwise.

Definition at line 256 of file mst_path.cpp.

◆ initialize_graph()

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

Initialize the internal graph structure that represents the area division.

Parameters
graphgridmap The grid map for which the paths are generated.
connect4Whether only the von Neumann neighborhood is considered. Default true.

Definition at line 152 of file mst_path.cpp.

◆ initialize_tree()

void mst_path::initialize_tree ( vector< edge mst)

Remove edges of the graph that overlap with the tree.

Parameters
mstThe edges that define the tree.

Definition at line 209 of file mst_path.cpp.

◆ remove_edge()

void mst_path::remove_edge ( edge  e)
private

Remove an edge from the tree graph.

Parameters
eThe edge to remove.

Definition at line 267 of file mst_path.cpp.

◆ valid()

bool mst_path::valid ( )

Check whether the current waypoint index is valid.

Returns
True, if the waypoint index is within the limits of the path, false otherwise.

Definition at line 235 of file mst_path.cpp.

Member Data Documentation

◆ edges

unordered_set<edge, hash_edge> mst_path::edges
private

Priority queue of edge objects sorted by cost.

Definition at line 112 of file mst_path.h.

◆ map

nav_msgs::OccupancyGrid mst_path::map
private

The grid map that needs to be covered by the MST path.

Definition at line 122 of file mst_path.h.

◆ nodes

vector<unordered_set<int> > mst_path::nodes
private

The sets of connected vertices.

Definition at line 117 of file mst_path.h.

◆ path

deque<geometry_msgs::Point> mst_path::path
private

The boustrophedon path.

Definition at line 107 of file mst_path.h.

◆ wp

int mst_path::wp
private

The current way point.

Definition at line 127 of file mst_path.h.


The documentation for this class was generated from the following files:


coverage_path
Author(s): Micha Sende
autogenerated on Thu Oct 31 2019 10:37:31