src/RAstar.cpp
/******************************************************************************/
// Function Name: constructPath
// Inputs: the start and the goal Cells
// Output: the best path
// Description: it is used to construct the robot path
/******************************************************************************/
Function Name: constructPath
Inputs: the start and the goal Cells
Output: the best path
Description: it is used to construct the robot path
Function Name : constructPath Inputs : the start and the goal Cells Output : the best path Description : it is used to construct the robot path
vector<int> RAstarPlannerROS::constructPath(int startCell, int goalCell,
                                            float g_score[]) {
  vector<int> bestPath;
  vector<int> path;
  path.insert(path.begin() + bestPath.size(), goalCell);
  int currentCell = goalCell;
  while (currentCell != startCell) {
    vector<int> neighborCells;
    neighborCells = findFreeNeighborCell(currentCell);
    vector<float> gScoresNeighbors;
    for (uint i = 0; i < neighborCells.size(); i++)
      gScoresNeighbors.push_back(g_score[neighborCells[i]]);
    int posMinGScore =
        distance(gScoresNeighbors.begin(),
                 min_element(gScoresNeighbors.begin(), gScoresNeighbors.end()));
    currentCell = neighborCells[posMinGScore];
    path.insert(path.begin() + path.size(), currentCell);
  }
  for (uint i = 0; i < path.size(); i++)
    bestPath.insert(bestPath.begin() + bestPath.size(),
                    path[path.size() - (i + 1)]);
  return bestPath;
}
vector < int > RAstarPlannerROS :: constructPath ( int startCell , int goalCell , float g_score [ ] ) { vector < int > bestPath ; vector < int > path ; path . insert ( path . begin ( ) + bestPath . size ( ) , goalCell ) ; int currentCell = goalCell ; while ( currentCell != startCell ) { vector < int > neighborCells ; neighborCells = findFreeNeighborCell ( currentCell ) ; vector < float > gScoresNeighbors ; for ( uint i = 0 ; i < neighborCells . size ( ) ; i ++ ) gScoresNeighbors . push_back ( g_score [ neighborCells [ i ] ] ) ; int posMinGScore = distance ( gScoresNeighbors . begin ( ) , min_element ( gScoresNeighbors . begin ( ) , gScoresNeighbors . end ( ) ) ) ; currentCell = neighborCells [ posMinGScore ] ; path . insert ( path . begin ( ) + path . size ( ) , currentCell ) ; } for ( uint i = 0 ; i < path . size ( ) ; i ++ ) bestPath . insert ( bestPath . begin ( ) + bestPath . size ( ) , path [ path . size ( ) - ( i + 1 ) ] ) ; return bestPath ; }
Function Name: constructPath
Inputs: the start and the goal Cells
Output: the best path
Description: it is used to construct the robot path
Function Name : constructPath Inputs : the start and the goal Cells Output : the best path Description : it is used to construct the robot path
// insert the neighbor in the path // publishPlan(bestPath, 0.0, 1.0, 0.0, 0.0);