void Manager::calculateWayPoint(){
vector<Waypoint> vecWaypoints;
Graph gr = _map->GetGraphFromGrid();
AStar a;
Node* start = gr.GetNodeByPos(_slam->BestPrticle()->GetX(), _slam->BestPrticle()->GetY());
Node* end = gr.GetNodeByPos(40, 47);
vector<Node*> vec = a.GetShortestPath(start, end);
for (int i = vec.size() - 1; i > 0; --i) {
Waypoint wp(vec.operator [](i)->getXPos(),vec.operator [](i)->getYPos());
cout << "AStar path: (" << wp.getX() << "," << wp.getY() << ")" << endl;
vecWaypoints.push_back(wp);
}
cout << "Finished AStar path" << endl;
this->_robot->SetWaypointVector(vecWaypoints);
}
Graph MapHelp::GetGraphFromGrid() {
cout << "start Grid to Graph" << endl;
vector<Waypoint> way;
Graph gr;
int NodeID = 0;
for (int i = 0; i < this->GetBlownGridMap()->GetHeight(); ++i) {
for (int j = 0; j < this->GetBlownGridMap()->GetWidth(); ++j) {
gr.InsertVertex(NodeID, 5, j, i, false);
// cout << "node " << NodeID << " x: " << i << " y: " << j
// << " value: " << this->GetBlownGridMap()->GetValue(j, i)
// << endl;
NodeID++;
}
}
for (int i = 0; i < this->GetBlownGridMap()->GetHeight(); ++i) {
for (int j = 0; j < this->GetBlownGridMap()->GetWidth(); ++j) {
// For each cell:
int ij = this->GetBlownGridMap()->GetValue(j, i);
if (ij == 0) {
// Iterate over the closest neighbours 3x3 matrix:+s
for (int k = i - 1; k <= i + 1; k++) {
for (int m = j - 1; m <= j + 1; m++) {
// node name: k,m
//
if (k < 0 || m < 0
|| k >= this->GetBlownGridMap()->GetHeight()
|| m >= this->GetBlownGridMap()->GetWidth()) {
} else {
int km = this->GetBlownGridMap()->GetValue(m, k);//[k][m];
//blownMapMatrix[k][m]
//if (node [k,m]=white AND [i,j]=white AND [k,m]!=[i,j]) -> add edge
if (km == 0 && (k != i || m != j)) {
//add edge
// from ij to km
//gr.InsertEdge(gr.GetNodeByPos(i, j)->GetData(), gr.GetNodeByPos(k, m)->GetData(), 1);
// cout << "Parent: " << i << " " << j << " Son: "
// << k << " " << m << endl;
gr.AddSuccessor(
gr.GetNodeByPos(j, i)->GetData(),
gr.GetNodeByPos(m, k)->GetData());
// cout << gr.GetNodeByPos(j, i)->GetSuccessors().size() << endl;
}
}
}
}
}
}
}
cout << "Finished Grid to Graph" << endl;
return gr;
}