TASK 1: LOCALIZED VACCUM CLEANER.

 INTRODUCTION

The objective of this practice is to implement the logic of a navigation algorithm for an autonomous vacuum cleaner by making use of robot's location. The robot is equipped with a map and knows its current location. The main objective is to cover the largest area of a house using the algorithm designed.

Along this explanation, I am going to cover the following topics: 

• Map
• Set scenario and Cell Grid 
• Location 
• Planning: BSA and BFS
• Navigation
• Conclusion
• Video
  

 

MAP

To start handling this task, it is important to know that the robot's map is stored in: /RoboticsAcademy/exercises/static/exercises/vacuum_cleaner_loc_newmanager/resources/mapgrannyannie.png

And it is shown like this:

 Also, beware that the map needs a conversion from R,G,B,A float map to another one with R,G,B int values. 

 To ensure that the robot will not collide with any obstacle, it is important to dilate black part of the image. However, if you use the dilate function provided by cv2, it will expand the white part. So, you will need to use the erode function, also provided by cv2. Here, you can see the differences:

- Using dilate:

 

-  Using erode: 

 

 

SET SCENARIO AND CELL GRID:

Set scenario section, refers when it is important to prepare the map before location. 

To achieve this, I need the robot dimension to divide the map in different cells. As we do not know the exact values, I need to guess it and I did it placing the robot in different places in the gazebo world. 

 

Using the functions to get the robot coordinates, I realized that the distance of the door was around 0.65 and two robots can be placed there, so I finally decided to say that the robot dimension is 32x32. Also, I decided to set the cell dimension also as 32x32; so a cell will be ticked as cleaned as the robot will cover the whole part.

The original size of the map was 1012x1013 and by display advice, I had to resize it to a value less than 1000x1000. So, I decided to set the image dimension up to 512x512 since it is around half of the original image and all cells will be 16x16 pixels. 

Besides, to make it robust, it is important that if in any cell exist any black pixel, the whole cell should be set as a obstacle.

This is how it looks with the grid, made by rectangles: 

 

LOCATION:

Now, the next step to do is to make relations with the 3 different dimensions that we will be working with:  pixel, meter and cell.

And to make it work I decided to use linear regression. 

Here you can see all the points I decided to take and its different conversions: 

• Blue: refers to the meter
• Green: refers to the cell
• Orange: refers to the up-left pixel

 

 

Getting these points, I have done, 2 relations: 

 Meter -> Cell  

The equations obtained are: 

x_2d = 17.963 - 2.915*x_3d

  y_2d = 13.959 + 3.092*y_3d 

 Pixel -> Meter

The equations obtained are: 

  x_meter = 5.841 - 0.021*x_pixel 

  y_meter = -4.671 + 0.02*y_pixel

 

Finally, in my final implementation I just use the conversion from pixel to meter for the navigation part. The other one helped me a lot to get the other and helped me debugging. 

 

PLANNING

• BSA (backtracking spiral algorithm):
  

After knowing that the vacuum is localized, it is the moment to plan the BSA algorithm. 

The idea of BSA algorithm is to start from a certain point in the grid and navigate through it in a spiral manner, always using the same order of orientations. Mine are: 

EAST -> NORTH -> WEST -> SOUTH 

I have followed the next steps: 

1. Start at the initial position in the grid.

2. Move in the current direction:

   • If the cell you're moving to is within the grid boundaries and has not been visited, move to that cell and mark it as visited.
   • If the cell is outside the boundaries or has already been visited, change direction (e.g., from right to down, down to left, left to up, up to right) and continue.

3. Continue moving in the new direction, repeating step 3, until you reach a cell that either:

   • Is outside the grid boundaries.
   • Has already been visited.
     

After you get the first spiral (means you have reached to a point of no return), it is necessary to do it for the rest of white cells. So, to achieve that, it is necessary to store neighbors cells to check later where is the closest neighbor. I have done it using BFS. 

• BFS (Breadth-First Search):
   

Is a graph traversal algorithm used to explore and analyze the structure of a graph or tree. It operates in a systematic manner, starting from a specified node and visiting all of its neighbors before moving on to their neighbors, effectively moving layer by layer. Here's how BFS works:

1. Initialization:

   • Choose a starting node, often called the "source" or "root" node.
   • Create a queue (FIFO - First In, First Out) to keep track of nodes to visit.
   • Mark the starting node as visited and enqueue it.

2. Exploration:

   • While the queue is not empty, repeat the following steps.
   • Dequeue a node from the front of the queue. This node will be the current node.
   • Explore all unvisited neighbors of the current node.
   • For each unvisited neighbor:
     • Mark it as visited.
     • Enqueue it in the queue.

3. Repeat:

   • Continue this process until the queue is empty. This ensures that you visit all nodes reachable from the starting node.
     

It needs an heuristic for making all calculations, so I decided to use the euclidean distance.

 Here you can see a graphical description of how the euclidean distance works: 

To make things clearer, down below, you can see how my BSA and BFS grafically works. It is necessary to know that: 

• Purple: used for indicating east direction 
• Yellow: used for indicating north direction 
• Green: used for indicating west direction
• Blue: used for indicating south direction 
• Red: used for indicating possible neighbors 

NAVIGATION

At this point, the only thing left to do is implementing the navigation part. 

At the beginning, I found it really difficult to manage since my first idea was storing each cell direction and check if my robot was going on the right direction. However, it was quite confusing checking the direction since yaw function and atan2 have different values. Moreover, when my robot reached to a corner, turned before moving one cell and moving one cell was complicated to program due to thereactivity of the system.

 Finally, what I decided to implement is : using the conversion from the navigation pixel of a cell into meters (defined before), I take all the time into account the distance from the objective to the actual position in meters.

With that differences I calculate the atan and I compare it with my current angle and if that difference is big (means that the angle is no close to the objective), the robot turns. And if the difference is small (means that you are aligned to the objective), the robot goes ahead. 

To improve the navigation, I have included a proportional controller to help with the turn movement. 

 

VIDEO

Here you can see my final result. Every time you see a new cell painted is the cell objective.

It took me around 4 hours and a half to complete the whole scenario due to I had to try it in remote mode and my computer lacks of graphical processing. I modified the video into a x16 to be able to uploaded.

In 9 and 12:30 minutes you can see a cut since my recorder stopped working. I hope it does not matter.

CONCLUSION

I think this task has been quite tough to handle. However, it has been an interesting challenge since you can get to know how sophisticated autonomous vacuum cleaners work.