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 ROSWARN. I tried to debug the plugin with some ROSDEBUG 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 doesn't work because it's not able to recognize the wall!!!
MAP: link text
EDIT:This is the output when i select "far" goal
process[robotstatepublisher-1]: started with pid [7925]
process[map_server-2]: started with pid [7926]
process[amcl-3]: started with pid [7927]
process[move_base-4]: started with pid [7928]
[ INFO] [1527958581.902716425, 182.172000000]: Using plugin "static_layer"
[ INFO] [1527958581.928160221, 182.184000000]: Requesting the map...
[ INFO] [1527958582.323335265, 182.386000000]: Resizing costmap to 384 X 384 at 0.050000 m/pix
[ INFO] [1527958582.497469604, 182.486000000]: Received a 384 X 384 map at 0.050000 m/pix
[ INFO] [1527958582.518370838, 182.499000000]: Using plugin "obstacle_layer"
[ INFO] [1527958582.523065013, 182.499000000]: Subscribed to Topics:
[ INFO] [1527958582.570230017, 182.522000000]: Using plugin "inflation_layer"
[DEBUG] [1527958582.717763200, 182.580000000]: Sono in DEBUG mode
[ INFO] [1527958582.719209475, 182.580000000]: RAstar planner initialized successfully
[ INFO] [1527958582.739209612, 182.586000000]: Using plugin "obstacle_layer"
[ INFO] [1527958582.744542603, 182.587000000]: Subscribed to Topics:
[ INFO] [1527958582.784379476, 182.609000000]: Using plugin "inflation_layer"
[ INFO] [1527958582.928199838, 182.687000000]: Created localplanner dwalocal_planner/DWAPlannerROS
[ INFO] [1527958582.937675689, 182.691000000]: Sim period is set to 1.00
[ INFO] [1527958583.757779282, 183.180000000]: Recovery behavior will clear layer obstacles
[ INFO] [1527958583.775290018, 183.185000000]: Recovery behavior will clear layer obstacles
[ INFO] [1527958583.898702887, 183.241000000]: odom received!
Click on 2D Nav Goal "far" point (another room)
[DEBUG] [1527958637.245100396, 216.689000000]: Failure to find a path !
time to generate best global path by Relaxed A* = 18.6031 microseconds
[ WARN] [1527958637.245631963, 216.689000000]: The planner failed to find a path, choose other goal position
[DEBUG] [1527958637.987958437, 217.187000000]: Failure to find a path !
time to generate best global path by Relaxed A* = 16.4637 microseconds
[ WARN] [1527958637.988185245, 217.187000000]: The planner failed to find a path, choose other goal position
[DEBUG] [1527958638.777376567, 217.684000000]: Failure to find a path !
time to generate best global path by Relaxed A* = 13.7155 microseconds
[ WARN] [1527958638.777749366, 217.684000000]: The planner failed to find a path, choose other goal position
[ WARN] [1527958640.314609101, 218.678000000]: Clearing costmap to unstuck robot (0.100000m).
[DEBUG] [1527958641.873638529, 219.693000000]: Failure to find a path !
time to generate best global path by Relaxed A* = 19.0566 microseconds
[ WARN] [1527958641.874017256, 219.694000000]: The planner failed to find a path, choose other goal position
[ WARN] [1527958643.373685972, 220.677000000]: Rotate recovery behavior started.
[DEBUG] [1527958644.963501703, 221.688000000]: Failure to find a path !
time to generate best global path by Relaxed A* = 14.5358 microseconds
[ WARN] [1527958644.963737474, 221.688000000]: The planner failed to find a path, choose other goal position
[DEBUG] [1527958645.806813329, 222.181000000]: Failure to find a path !
time to generate best global path by Relaxed A* = 14.5959 microseconds
[ WARN] [1527958645.807084293, 222.181000000]: The planner failed to find a path, choose other goal position
[ WARN] [1527958646.679014050, 222.678000000]: Clearing costmap to unstuck robot (1.840000m).
[DEBUG] [1527958648.378683250, 223.690000000]: Failure to find a path !
time to generate best global path by Relaxed A* = 16.06 microseconds
[ WARN] [1527958648.379011351, 223.690000000]: The planner failed to find a path, choose other goal position
[ WARN] [1527958650.031918266, 224.678000000]: Rotate recovery behavior started.
[DEBUG] [1527958651.622007210, 225.689000000]: Failure to find a path !
time to generate best global path by Relaxed A* = 14.7284 microseconds
[ WARN] [1527958651.622258492, 225.689000000]: The planner failed to find a path, choose other goal position
[DEBUG] [1527958652.500072967, 226.182000000]: Failure to find a path !
time to generate best global path by Relaxed A* = 16.6565 microseconds
[ WARN] [1527958652.501536073, 226.183000000]: The planner failed to find a path, choose other goal position
[ERROR] [1527958653.248970004, 226.677000000]: Aborting because a valid plan could not be found. Even after executing all recovery behaviors
Asked by kenhero on 2018-05-31 16:07:02 UTC
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.
Asked by PeteBlackerThe3rd on 2018-06-01 06:29:33 UTC
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
Asked by kenhero on 2018-06-01 06:52:45 UTC
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.
Asked by PeteBlackerThe3rd on 2018-06-01 10:52:05 UTC
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?
Asked by PeteBlackerThe3rd on 2018-06-03 12:15:30 UTC
link text
Asked by kenhero on 2018-06-03 20:15:06 UTC
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.
Asked by PeteBlackerThe3rd on 2018-06-04 08:13:57 UTC
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
Asked by kenhero on 2018-06-04 08:23:20 UTC
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.
Asked by PeteBlackerThe3rd on 2018-06-04 08:54:24 UTC
Again, I'm fairly sure your problem is caused by the maps not being setup properly. I don't think there is a bug in this A* algorithm.
Asked by PeteBlackerThe3rd on 2018-06-04 08:54:59 UTC
when you talking about threshold do you intend ,for example:
inside gmapping package?
Btw if i run rostopic echo /map i can see lot of locations with -1...probably i missed something when i created the map
Asked by kenhero on 2018-06-04 09:55:58 UTC
@PeterBlackerThe3rd could you link me doc about how to setup map?
Asked by kenhero on 2018-06-05 08:10:21 UTC