14 geometry_msgs::PoseArray
path;
15 vector<geometry_msgs::Pose> poses;
16 geometry_msgs::Pose pose;
20 pose.position.x = e.from %
map.info.width *
map.info.resolution +
map.info.origin.position.x;
21 pose.position.y = e.from /
map.info.width *
map.info.resolution +
map.info.origin.position.y;
24 double dx = e.to %
map.info.width *
map.info.resolution +
map.info.origin.position.x - pose.position.x;
25 double dy = e.to /
map.info.width *
map.info.resolution +
map.info.origin.position.y - pose.position.y;
27 direction.
setRPY(0, 0, atan2(dy, dx));
30 poses.push_back(pose);
34 path.header.stamp = Time::now();
35 path.header.frame_id =
"local_origin_ned";
46 int rows = gridmap.info.height;
47 int cols = gridmap.info.width;
51 nodes.resize(rows*cols);
55 for (
int i=0; i<rows; i++) {
56 for (
int j=0; j<cols; j++) {
58 if (
map.data[i*cols+j] == 0) {
60 if (i>0 &&
map.data[(i-1)*cols+j] == 0) {
63 if (i<rows-1 &&
map.data[(i+1)*cols+j] == 0) {
66 if (j>0 &&
map.data[i*cols+j-1] == 0) {
69 if (j<cols-1 &&
map.data[i*cols+j+1] == 0) {
75 if (i>0 && j>0 &&
map.data[(i-1)*cols+j-1] == 0) {
76 add_edge(i*cols+j, (i-1)*cols+j-1, 1);
78 if (i<rows-1 && j<cols-1 &&
map.data[(i+1)*cols+j+1] == 0) {
79 add_edge(i*cols+j, (i+1)*cols+j+1, 1);
81 if (i<rows-1 && j>0 &&
map.data[(i+1)*cols+j-1] == 0) {
82 add_edge(i*cols+j, (i+1)*cols+j-1, 1);
84 if (i>0 && j<cols-1 &&
map.data[(i-1)*cols+j+1] == 0) {
85 add_edge(i*cols+j, (i-1)*cols+j+1, 1);
97 while (!
edges.empty()) {
125 if (
nodes[from].size() == 0) {
126 nodes[from].insert(from);
128 if (
nodes[to].size() == 0) {
129 nodes[to].insert(to);
nav_msgs::OccupancyGrid map
The grid map that needs to be covered by the MST.
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
vector< edge > mst_edges
Edges in Minimal-Spanning Tree.
void add_edge(int from, int to, int cost)
Add an edge to the tree.
void construct()
Generate the MST using Kruskal's algorithm.
A struct that provides the comparison of edge objects. It allows sorting of edges for priority queues...
vector< edge > get_mst_edges()
Get the edges of the MST.
int to
The ending vertex of the edge.
A class for representing edges.
int from
The starting vertex of the edge.
spanning_tree()
Constructor.
void initialize_graph(nav_msgs::OccupancyGrid gridmap, bool vertical=false, bool connect4=true)
Initialize the internal tree structure from a given grid map.
mst_path path
The coverage path.
geometry_msgs::PoseArray get_tree()
Get the generated MST for visualization.
vector< unordered_set< int > > nodes
The sets of connected vertices.
bool vertical
Whether the sweeping pattern is vertical or horizontal.
priority_queue< edge, vector< edge >, compare_edge > edges
Priority queue of edge objects sorted by cost.
bool different_sets(int a, int b)
Check whether two vertices are in different sets.