View unanswered posts | View active topics It is currently Wed Sep 17, 2014 7:32 am






Reply to topic  [ 5 posts ] 
Path Planning--Wave front algorithm 
Author Message
Moderator
Moderator

Joined: Mon Oct 04, 2010 2:18 pm
Posts: 196
Post Path Planning--Wave front algorithm
My students are currently working through the lessons on the multi-robot communications wik. They have completed the relay race challenge, and are currently working on the seeing eye robot lab. In a little while, I'd like for them to be able to do the autonomous path planning that is detailed here: http://www.robotc.net/blog/2011/08/08/robotc-advanced-training/

So, I'm trying to go through the code myself now in order to teach the students, and I'm in need of a little help. My first question involves the function that is actually performing the wavefront algorithm. Here is the code
Code:
//FUNCTION wavefront algorithm to find most efficient path to goal
void WavefrontSearch()
{
  int goal_x, goal_y;
  bool foundWave = true;
  int currentWave = 2; //Looking for goal first
 
  while(foundWave == true)
  {
    foundWave = false;
    for(int y=0; y < y_size; y++)
    {
      for(int x=0; x < x_size; x++)
      {
        if(map[x][y] == currentWave)
        {
          foundWave = true;
          goal_x = x;
          goal_y = y;
 
          if(goal_x > 0) //This code checks the array bounds heading WEST
            if(map[goal_x-1][goal_y] == 0)  //This code checks the WEST direction
              map[goal_x-1][goal_y] = currentWave + 1;
 
          if(goal_x < (x_size - 1)) //This code checks the array bounds heading EAST
            if(map[goal_x+1][goal_y] == 0)//This code checks the EAST direction
              map[goal_x+1][goal_y] = currentWave + 1;
 
          if(goal_y > 0)//This code checks the array bounds heading SOUTH
            if(map[goal_x][goal_y-1] == 0) //This code checks the SOUTH direction
              map[goal_x][goal_y-1] = currentWave + 1;
 
          if(goal_y < (y_size - 1))//This code checks the array bounds heading NORTH
            if(map[goal_x][goal_y+1] == 0) //This code checks the NORTH direction
              map[goal_x][goal_y+1] = currentWave + 1;
        }
      }
    }
    currentWave++;
    PrintWavefrontMap();
    wait1Msec(500);
  }
}


At the beginning of the function, it creates a boolean variable called foundwave. It is then used as the condition in the while loop. But then, in the body of the loop, it is assigned a value of false. Further down in the body of the for loop, the variable foundwave is again used, and it is now assigned the value of true. If someone could explain to me the logic behind the use of this variable, I'd be appreciative.


Sun Oct 28, 2012 10:11 pm
Profile
Site Admin
Site Admin

Joined: Wed Jan 24, 2007 10:42 am
Posts: 602
Post Re: Path Planning--Wave front algorithm
So I'm the one who wrote the code... and I'm not admitting it's the easiest/best code out there.

The motivation was I needed to iterate through the loop to see if I could find the current wave that I'm looking for (i.e. a way to check that I'm not out of current spaces to iterate through). So I would set the variable to "false" at the beginning of each loop as I was iterating through each space... If I found a space that had the current value I was looking for, I would set the flag to "true" to indicate that I did complete some work this pass through the map and that I should iterate my searching number and allow myself to scan the map again.

Once I was done searching the map, I would know this because I scanned through the entire map without finding the value I was looking for, so the variable would remain "false" and I would then exit the function.

I hope this makes sense... let me know if it doesn't and I'll try and dig up some of my diagrams.

_________________
Timothy Friez
ROBOTC Developer - SW Engineer
tfriez@robotc.net


Mon Oct 29, 2012 3:52 pm
Profile
Moderator
Moderator
User avatar

Joined: Tue Sep 14, 2010 9:19 pm
Posts: 496
Post Re: Path Planning--Wave front algorithm
If you would like more information on the logic of path finding algorithms, I would suggest looking at depth first search and breadth first search, and then looking up Dijkstra's algorithm, and A* (A star).

_________________
sudo rm -rf /


Mon Oct 29, 2012 3:59 pm
Profile
Moderator
Moderator

Joined: Mon Oct 04, 2010 2:18 pm
Posts: 196
Post Re: Path Planning--Wave front algorithm
Thank you for the response. Here is my next question. I'm confused with this line:

if(goal_x > 0) //This code checks the array bounds heading WEST
if(map[goal_x-1][goal_y] == 0) //This code checks the WEST direction
map[goal_x-1][goal_y] = currentWave + 1;

I think I understand what the code is supposed to accomplish. It is attempting to see if the adjacent cell is equal to zero. If so, its value is increased by one. Since the goal was 2, this would give the adjacent cell a value of 3. Thus, the wave begins. But, as I mentioned, I'm not sure how that is actually accomplished. Thanks again.


Tue Oct 30, 2012 8:10 pm
Profile
Site Admin
Site Admin

Joined: Thu May 24, 2012 12:15 pm
Posts: 575
Post Re: Path Planning--Wave front algorithm
Reposting this here for future reference:

The goal_x and goal_y integers are used to store the array location of the 'goal'. The foundWave bool starts off as true to make sure the first while loop gets entered; it is then immediately changed to false so the program will go through the for loops until it finds the goal (more on this soon). The currentWave integer is set to 2 because '2' is the value that the 'goal' location in the array holds.

The while loop continues until foundWave is false (which is, again, why it had to be set to true during initialization). The foundWave bool is immediately set to false, and then the for loops are entered. The nested for loops check each individual array location (starting at 0,0 it goes through each x 'column' in a y 'row' before moving on to the next one) until it finds the value of 2 (if (map[x][y] == currentWave), this is why currentWave had to be set to 2).

When the goal is found (and the if (map[x][y] == currentWave) become true), foundWave is set to true again and the goal_x and goal_y variables are set equal to the array location where the value of 2 was found. The next four nested if statements check the array locations to the 'West', 'East', 'South', and 'North' of the goal to see if they have a value of 0 (which means it is 'clear'; the robot start location has a value of 99, the goal location has a value of 2, and the 'obstacles' have a value of 1). If the value of the adjacent array is open, the array location is assigned a value of the current wave +1 (so from the goal location, the open adjacent locations will be assigned a value of 3).

Once this is complete and the rest of the array is checked for the goal location (which is has already found), the two 'for' loops complete and exit. The currentWave variable is increased by one (now holds a value of 3), the PrintWavefrontMap function is called (which updates the NXT display with the value stored in each array location), and there is a 500 ms pause to allow the NXT screen time to update itself. Remember, the foundWave boolean is still true from the time the program found the goal array location.

The program returns to the while loop, checks the condition (foundWave is true), and repeats the process, only this time it is looking for the value of 3 (which are located in the open array locations adjacent to the goal), and repeats the process, adding the number 4 to any open locations that are both adjacent to the 3 values and inside the bounds of the array. This process of increasing the currentWave variable and then rechecking the values is repeated until the entire array is filled with values. When the array is filled with values, the if(map[x][y] == currentWave) remains false (since the program will not fill values outside of the array range), the foundWave variable is not set to true, and the while loop will exit.

The PrintWavefrontMap() function simply checks each array location and prints a corresponding value to the NXT screen; 99 stands for the robot ('R'), 2 stands for the goal ('G'), 1 stands for obstacle ('X'), asterisks stand for locations the robot has already been ('*', set during the NavigateToGoal function), and the < 10 puts spaces in between the values for easier reading.

That is how the WavefrontSearch maps each 'square' in the array to a specific character value.

_________________
Check out our Blog! And our Facebook page!
Need help? Take a look at our Wiki and our Forums.

I just met you,
And this is crazy,
But here's my code now,
So fix it, maybe?
~ Carly Rae Jepsen parody


Thu Nov 01, 2012 9:21 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 5 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.