ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Tue, 05 Jun 2018 08:10:21 -0500A star algorithm doesn't work on gazebo househttps://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/I used [relaxed A star algorithm](https://github.com/coins-lab/relaxed_astar) 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 doesn't work because it's not able to recognize the wall!!!
MAP:
[link text](https://ibb.co/nGwPFy)
EDIT:This is the output when i select "far" goal
> process[robot_state_publisher-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 local_planner dwa_local_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 behaviorsThu, 31 May 2018 16:07:02 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/Comment by kenhero for <div class="snippet"><p>I used <a href="https://github.com/coins-lab/relaxed_astar">relaxed A star algorithm</a> 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.</p>
<p>Basically the method that failes to find the path or some paths is:</p>
<pre><code>vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
</code></pre>
<p>I put in a txt file this infos,only when this method fails to find a path:</p>
<pre><code>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();
}
</code></pre>
<p>Every time it cannot find a path g_score[goalCell] = infinity</p>
<pre><code>if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
</code></pre>
<p>Info on my txt file:</p>
<pre><code>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
</code></pre>
<p>The core of this method is basically this loop:</p>
<pre><code>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
</code></pre>
<p>So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell </p>
<p>This is the findFreeNeighborCell method inside RAstarPLANNERROS:</p>
<pre><code> 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;
}
</code></pre>
<p>Suggestions? How to solve it?
ps:Carrot Planner find the path but it still ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293187#post-id-293187when you talking about threshold do you intend ,for example:
<param name="resampleThreshold" value="0.5"/>
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 mapMon, 04 Jun 2018 09:55:58 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293187#post-id-293187Comment by PeteBlackerThe3rd for <div class="snippet"><p>I used <a href="https://github.com/coins-lab/relaxed_astar">relaxed A star algorithm</a> 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.</p>
<p>Basically the method that failes to find the path or some paths is:</p>
<pre><code>vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
</code></pre>
<p>I put in a txt file this infos,only when this method fails to find a path:</p>
<pre><code>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();
}
</code></pre>
<p>Every time it cannot find a path g_score[goalCell] = infinity</p>
<pre><code>if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
</code></pre>
<p>Info on my txt file:</p>
<pre><code>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
</code></pre>
<p>The core of this method is basically this loop:</p>
<pre><code>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
</code></pre>
<p>So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell </p>
<p>This is the findFreeNeighborCell method inside RAstarPLANNERROS:</p>
<pre><code> 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;
}
</code></pre>
<p>Suggestions? How to solve it?
ps:Carrot Planner find the path but it still ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293168#post-id-293168What 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.Mon, 04 Jun 2018 08:13:57 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293168#post-id-293168Comment by kenhero for <div class="snippet"><p>I used <a href="https://github.com/coins-lab/relaxed_astar">relaxed A star algorithm</a> 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.</p>
<p>Basically the method that failes to find the path or some paths is:</p>
<pre><code>vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
</code></pre>
<p>I put in a txt file this infos,only when this method fails to find a path:</p>
<pre><code>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();
}
</code></pre>
<p>Every time it cannot find a path g_score[goalCell] = infinity</p>
<pre><code>if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
</code></pre>
<p>Info on my txt file:</p>
<pre><code>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
</code></pre>
<p>The core of this method is basically this loop:</p>
<pre><code>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
</code></pre>
<p>So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell </p>
<p>This is the findFreeNeighborCell method inside RAstarPLANNERROS:</p>
<pre><code> 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;
}
</code></pre>
<p>Suggestions? How to solve it?
ps:Carrot Planner find the path but it still ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=292963#post-id-292963[link text](https://github.com/coins-lab/relaxed_astar)
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 wallsFri, 01 Jun 2018 06:52:45 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=292963#post-id-292963Comment by PeteBlackerThe3rd for <div class="snippet"><p>I used <a href="https://github.com/coins-lab/relaxed_astar">relaxed A star algorithm</a> 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.</p>
<p>Basically the method that failes to find the path or some paths is:</p>
<pre><code>vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
</code></pre>
<p>I put in a txt file this infos,only when this method fails to find a path:</p>
<pre><code>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();
}
</code></pre>
<p>Every time it cannot find a path g_score[goalCell] = infinity</p>
<pre><code>if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
</code></pre>
<p>Info on my txt file:</p>
<pre><code>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
</code></pre>
<p>The core of this method is basically this loop:</p>
<pre><code>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
</code></pre>
<p>So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell </p>
<p>This is the findFreeNeighborCell method inside RAstarPLANNERROS:</p>
<pre><code> 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;
}
</code></pre>
<p>Suggestions? How to solve it?
ps:Carrot Planner find the path but it still ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293178#post-id-293178Again, 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.Mon, 04 Jun 2018 08:54:59 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293178#post-id-293178Comment by kenhero for <div class="snippet"><p>I used <a href="https://github.com/coins-lab/relaxed_astar">relaxed A star algorithm</a> 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.</p>
<p>Basically the method that failes to find the path or some paths is:</p>
<pre><code>vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
</code></pre>
<p>I put in a txt file this infos,only when this method fails to find a path:</p>
<pre><code>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();
}
</code></pre>
<p>Every time it cannot find a path g_score[goalCell] = infinity</p>
<pre><code>if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
</code></pre>
<p>Info on my txt file:</p>
<pre><code>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
</code></pre>
<p>The core of this method is basically this loop:</p>
<pre><code>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
</code></pre>
<p>So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell </p>
<p>This is the findFreeNeighborCell method inside RAstarPLANNERROS:</p>
<pre><code> 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;
}
</code></pre>
<p>Suggestions? How to solve it?
ps:Carrot Planner find the path but it still ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293173#post-id-293173Could 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 freeNeighborCellsMon, 04 Jun 2018 08:23:20 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293173#post-id-293173Comment by PeteBlackerThe3rd for <div class="snippet"><p>I used <a href="https://github.com/coins-lab/relaxed_astar">relaxed A star algorithm</a> 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.</p>
<p>Basically the method that failes to find the path or some paths is:</p>
<pre><code>vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
</code></pre>
<p>I put in a txt file this infos,only when this method fails to find a path:</p>
<pre><code>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();
}
</code></pre>
<p>Every time it cannot find a path g_score[goalCell] = infinity</p>
<pre><code>if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
</code></pre>
<p>Info on my txt file:</p>
<pre><code>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
</code></pre>
<p>The core of this method is basically this loop:</p>
<pre><code>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
</code></pre>
<p>So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell </p>
<p>This is the findFreeNeighborCell method inside RAstarPLANNERROS:</p>
<pre><code> 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;
}
</code></pre>
<p>Suggestions? How to solve it?
ps:Carrot Planner find the path but it still ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=292981#post-id-292981That'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.Fri, 01 Jun 2018 10:52:05 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=292981#post-id-292981Comment by PeteBlackerThe3rd for <div class="snippet"><p>I used <a href="https://github.com/coins-lab/relaxed_astar">relaxed A star algorithm</a> 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.</p>
<p>Basically the method that failes to find the path or some paths is:</p>
<pre><code>vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
</code></pre>
<p>I put in a txt file this infos,only when this method fails to find a path:</p>
<pre><code>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();
}
</code></pre>
<p>Every time it cannot find a path g_score[goalCell] = infinity</p>
<pre><code>if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
</code></pre>
<p>Info on my txt file:</p>
<pre><code>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
</code></pre>
<p>The core of this method is basically this loop:</p>
<pre><code>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
</code></pre>
<p>So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell </p>
<p>This is the findFreeNeighborCell method inside RAstarPLANNERROS:</p>
<pre><code> 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;
}
</code></pre>
<p>Suggestions? How to solve it?
ps:Carrot Planner find the path but it still ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293177#post-id-293177This 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.Mon, 04 Jun 2018 08:54:24 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293177#post-id-293177Comment by PeteBlackerThe3rd for <div class="snippet"><p>I used <a href="https://github.com/coins-lab/relaxed_astar">relaxed A star algorithm</a> 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.</p>
<p>Basically the method that failes to find the path or some paths is:</p>
<pre><code>vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
</code></pre>
<p>I put in a txt file this infos,only when this method fails to find a path:</p>
<pre><code>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();
}
</code></pre>
<p>Every time it cannot find a path g_score[goalCell] = infinity</p>
<pre><code>if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
</code></pre>
<p>Info on my txt file:</p>
<pre><code>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
</code></pre>
<p>The core of this method is basically this loop:</p>
<pre><code>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
</code></pre>
<p>So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell </p>
<p>This is the findFreeNeighborCell method inside RAstarPLANNERROS:</p>
<pre><code> 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;
}
</code></pre>
<p>Suggestions? How to solve it?
ps:Carrot Planner find the path but it still ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293095#post-id-293095I'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?Sun, 03 Jun 2018 12:15:30 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293095#post-id-293095Comment by PeteBlackerThe3rd for <div class="snippet"><p>I used <a href="https://github.com/coins-lab/relaxed_astar">relaxed A star algorithm</a> 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.</p>
<p>Basically the method that failes to find the path or some paths is:</p>
<pre><code>vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
</code></pre>
<p>I put in a txt file this infos,only when this method fails to find a path:</p>
<pre><code>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();
}
</code></pre>
<p>Every time it cannot find a path g_score[goalCell] = infinity</p>
<pre><code>if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
</code></pre>
<p>Info on my txt file:</p>
<pre><code>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
</code></pre>
<p>The core of this method is basically this loop:</p>
<pre><code>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
</code></pre>
<p>So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell </p>
<p>This is the findFreeNeighborCell method inside RAstarPLANNERROS:</p>
<pre><code> 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;
}
</code></pre>
<p>Suggestions? How to solve it?
ps:Carrot Planner find the path but it still ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=292960#post-id-292960Do 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.Fri, 01 Jun 2018 06:29:33 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=292960#post-id-292960Comment by kenhero for <div class="snippet"><p>I used <a href="https://github.com/coins-lab/relaxed_astar">relaxed A star algorithm</a> 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.</p>
<p>Basically the method that failes to find the path or some paths is:</p>
<pre><code>vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
</code></pre>
<p>I put in a txt file this infos,only when this method fails to find a path:</p>
<pre><code>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();
}
</code></pre>
<p>Every time it cannot find a path g_score[goalCell] = infinity</p>
<pre><code>if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
</code></pre>
<p>Info on my txt file:</p>
<pre><code>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
</code></pre>
<p>The core of this method is basically this loop:</p>
<pre><code>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
</code></pre>
<p>So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell </p>
<p>This is the findFreeNeighborCell method inside RAstarPLANNERROS:</p>
<pre><code> 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;
}
</code></pre>
<p>Suggestions? How to solve it?
ps:Carrot Planner find the path but it still ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293269#post-id-293269@PeterBlackerThe3rd could you link me doc about how to setup map?Tue, 05 Jun 2018 08:10:21 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293269#post-id-293269Comment by kenhero for <div class="snippet"><p>I used <a href="https://github.com/coins-lab/relaxed_astar">relaxed A star algorithm</a> 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.</p>
<p>Basically the method that failes to find the path or some paths is:</p>
<pre><code>vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
</code></pre>
<p>I put in a txt file this infos,only when this method fails to find a path:</p>
<pre><code>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();
}
</code></pre>
<p>Every time it cannot find a path g_score[goalCell] = infinity</p>
<pre><code>if(g_score[goalCell] != infinity)
{
bestPath=constructPath(startCell, goalCell, g_score);
return bestPath;
}
</code></pre>
<p>Info on my txt file:</p>
<pre><code>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
</code></pre>
<p>The core of this method is basically this loop:</p>
<pre><code>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
</code></pre>
<p>So probably the method findFreeNeighborCell(currentCell) is not able to find ALL neighbors of current cell </p>
<p>This is the findFreeNeighborCell method inside RAstarPLANNERROS:</p>
<pre><code> 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;
}
</code></pre>
<p>Suggestions? How to solve it?
ps:Carrot Planner find the path but it still ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293113#post-id-293113[link text](https://ibb.co/icJsnd)Sun, 03 Jun 2018 20:15:06 -0500https://answers.ros.org/question/292880/a-star-algorithm-doesnt-work-on-gazebo-house/?comment=293113#post-id-293113