A star algorithm doesn't work on gazebo house
I used relaxed A star 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<int> 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 <int> neighborCells;
neighborCells=findFreeNeighborCell(currentCell);
for(uint i=0; i<neighborCells.size(); i++) //for each neighbor v of current cell
{
// if the g_score of the neighbor is equal to INF: unvisited cell
if(g_score[neighborCells[i]]==infinity)
{
g_score[neighborCells[i]]=g_score[currentCell]+getMoveCost(currentCell,neighborCells[i]);
addNeighborCellToOpenList(OPL, neighborCells[i], goalCell, g_score);
}//end if
}//end for
}//end while
So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell
This is the findFreeNeighborCell method inside RAstarPLANNERROS:
vector <int> RAstarPlannerROS::findFreeNeighborCell (int CellID){
int rowID=getCellRowID(CellID);
int colID=getCellColID(CellID);
int neighborIndex;
vector <int> 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<height)&&(colID+j>=0)&&(colID+j<width)&& (!(i==0 && j==0))){
neighborIndex = getCellIndex(rowID+i,colID+j);
if(isFree(neighborIndex) )
freeNeighborCells.push_back(neighborIndex);
}
}
return freeNeighborCells;
}
Suggestions? How to solve it? ps:Carrot Planner find the path but it still ...
Do you mean the A* (A star) algorithm? Can you link to the github repo you are using? It would be helpful if you showed us the Warning messages you received, instead of just saying there were warning. Can you post the map too, it's possible the it is incomplete so that no route can be found.
link text The map is turtlebot3 house world. Later i try to upload the image but I think map is good. Carrot planner doesn't work because after a while it breaks through the walls
That's great, I've added it to your question now. Can you add the content of the ROS_WARN message you get when you try and plan a long route to your question. Thanks.
I've just had a look at the code and the warnings you're getting. I can't see anything immediately wrong, it appears as though there simply isn't a route to the goal point. Can you show where the start and goal points are on the map you've linked to?
link text
What threshold are you using to convert the gazebo house map into an obstacle map? I ask because there is no clear path to your goal if the threshold is set very high. The narrow corridor section is a slightly dark grey. I don't think A* is the problem here.
Could you explain what is it for
Why it has 2 for cycles from -1 to +1 ? Because after debugging the issue is inside this method and when a goal point is too "far" it returns 0 freeNeighborCells
This function checks the 8 adjacent map cells to see if they're free. It does this using two loops -1 to 1 and an if statement that checks the point is within the map area and that it isn't in the middle. This looks all correct to me.