I used relaxed A start algorithm for ROS found on github and used it on gazebo turtlebot3 simulation and house world.
Basically it works but only with goal close to start point .
If i try to select a goal from another room (for example) and turtlebot3 doesn't move with lots of ROS_WARN.
I tried to debug the plugin with some ROS_DEBUG messages and put in a txt file some infos in order to understand what happened when i select a goal too "far" from start but it's not easy.
Basically the method that failes to find the path or some paths is:
vector RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
I put in a txt file this infos,only when this method fails to find a path:
ofstream myfile ("gscorefile.txt", ios::app);
if(myfile.is_open())
{
myfile << "Start Cell = " << startCell << std::endl;
myfile << "Goal Cell = " << goalCell << std::endl;
myfile << "gscore[goalCell] = " << g_score[goalCell] << std::endl;
myfile << "gscore[startCell] = " << g_score[startCell] << std::endl;
myfile.close();
}
Every time it cannot find a path g_score[goalCell] = infinity
if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
Info on my txt file:
Start Cell = 84236
Goal Cell = 98949
gscore[goalCell] = inf
gscore[startCell] = 0
Start Cell = 84235
Goal Cell = 101252
gscore[goalCell] = inf
gscore[startCell] = 0
Start Cell = 84235
Goal Cell = 101252
gscore[goalCell] = inf
gscore[startCell] = 0
Start Cell = 84235
Goal Cell = 101252
gscore[goalCell] = inf
gscore[startCell] = 0
Start Cell = 84235
Goal Cell = 101252
gscore[goalCell] = inf
gscore[startCell] = 0
Start Cell = 84618
Goal Cell = 101252
gscore[goalCell] = inf
gscore[startCell] = 0
Start Cell = 84618
Goal Cell = 101252
gscore[goalCell] = inf
gscore[startCell] = 0
The core of this method is basically this loop:
while (!OPL.empty()&& g_score[goalCell]==infinity)
{
//choose the cell that has the lowest cost fCost in the open set which is the begin of the multiset
currentCell = OPL.begin()->currentCell;
//remove the currentCell from the openList
OPL.erase(OPL.begin());
//search the neighbors of the current Cell
vector neighborCells;
neighborCells=findFreeNeighborCell(currentCell);
for(uint i=0; i RAstarPlannerROS::findFreeNeighborCell (int CellID){
int rowID=getCellRowID(CellID);
int colID=getCellColID(CellID);
int neighborIndex;
vector freeNeighborCells;
for (int i=-1;i<=1;i++)
for (int j=-1; j<=1;j++){
//check whether the index is valid
if ((rowID+i>=0)&&(rowID+i=0)&&(colID+j
↧