A star algorithm doesn't work on gazebo house

edit

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)