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_edge > | edges |
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... | |
An class to compute the coverage path based on a minimum-spanning-tree (MST).
Definition at line 21 of file mst_path.h.
mst_path::mst_path | ( | ) |
Constructor.
Definition at line 3 of file mst_path.cpp.
|
private |
Add an edge to the tree graph.
from | The starting vertex. |
to | The ending vertex. |
cost | The cost of the edge. |
Definition at line 240 of file mst_path.cpp.
|
private |
Calculate the distance between two points.
p1 | The first point. |
p2 | The second point. |
Definition at line 251 of file mst_path.cpp.
void mst_path::generate_path | ( | geometry_msgs::Point | start | ) |
Generate all way points for the path.
start | The current position of the CPS. |
Definition at line 7 of file mst_path.cpp.
geometry_msgs::PoseArray mst_path::get_grid | ( | ) |
Get the graph generated from the grid map.
Definition at line 97 of file mst_path.cpp.
nav_msgs::Path mst_path::get_path | ( | ) |
Get the complete path.
Definition at line 126 of file mst_path.cpp.
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.
position | The current position of the CPS. |
tolerance | The distance to the current waypoint below which the next waypoint is selected. |
Definition at line 141 of file mst_path.cpp.
|
private |
Get the current way point.
offset | The index offset to the current waypoint. |
Definition at line 256 of file mst_path.cpp.
void mst_path::initialize_graph | ( | nav_msgs::OccupancyGrid | gridmap, |
bool | connect4 = true |
||
) |
Initialize the internal graph structure that represents the area division.
graph | gridmap The grid map for which the paths are generated. |
connect4 | Whether only the von Neumann neighborhood is considered. Default true. |
Definition at line 152 of file mst_path.cpp.
void mst_path::initialize_tree | ( | vector< edge > | mst | ) |
Remove edges of the graph that overlap with the tree.
mst | The edges that define the tree. |
Definition at line 209 of file mst_path.cpp.
|
private |
Remove an edge from the tree graph.
e | The edge to remove. |
Definition at line 267 of file mst_path.cpp.
bool mst_path::valid | ( | ) |
Check whether the current waypoint index is valid.
Definition at line 235 of file mst_path.cpp.
Priority queue of edge objects sorted by cost.
Definition at line 112 of file mst_path.h.
|
private |
The grid map that needs to be covered by the MST path.
Definition at line 122 of file mst_path.h.
|
private |
The sets of connected vertices.
Definition at line 117 of file mst_path.h.
|
private |
The boustrophedon path.
Definition at line 107 of file mst_path.h.
|
private |
The current way point.
Definition at line 127 of file mst_path.h.