A star algorithm doesn't work on gazebo house

asked 2018-05-31 16:07:02 -0600

kenhero gravatar image

updated 2018-06-03 08:59:04 -0600

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 ... (more)

edit retag flag offensive close merge delete

Comments

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.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-06-01 06:29:33 -0600 )edit

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

kenhero gravatar imagekenhero ( 2018-06-01 06:52:45 -0600 )edit

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.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-06-01 10:52:05 -0600 )edit

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?

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-06-03 12:15:30 -0600 )edit
kenhero gravatar imagekenhero ( 2018-06-03 20:15:06 -0600 )edit

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.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-06-04 08:13:57 -0600 )edit

Could you explain what is it for

vector <int> RAstarPlannerROS::findFreeNeighborCell (int CellID)

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

kenhero gravatar imagekenhero ( 2018-06-04 08:23:20 -0600 )edit

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.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-06-04 08:54:24 -0600 )edit