Add more comment and documentation

Signed-off-by: Thomas Bonnefille <thomas.bonnefille@outlook.com>

README.md

@@ -8,6 +8,15 @@ La recherche de Chemin se fait par l'algorithme **P\***  (Pepino \*), cette algo
 
 L'algorithme part d'un tableau en 2D, ce tableau sera mis à jour avec le meilleur chemin pour accéder à chacune des cases, on reparcourt le tableau tant que le tableau des distances d'accès varie.
 
+#### Errno
+
+Ci-dessous voici la liste des erreurs que peut renvoyer l'algorithme de recherche de chemin : 
+	- Errno 0  : Pas d'erreur
+	- Errno -1 : Pas de chemin valide trouvé
+	- Errno -2 : Recherche d'un chemin où l'on commence dans un obstacle
+	- Errno -3 : Recherche de chemin où l'on fini dans un obstacle
+
+
 ### Mise à jour du chemin
 
 Le chemin est mis à jour (c'est à dire recalculé) si et seulement si :
@@ -71,4 +80,4 @@ Ce noeud dispose des Publisher suivants :
 | --------- | ---------- | ----------- |
 | set_map | map : tabelau 2D du plateau false si ... et true si ... | recopie la map fournie et créé une map des couts (cout maximums partout) |
 | get_cost | p : point du tableau dont on veux calculer le cout | Calcule le cout d'un point du tableau a partir du cout des 8 cases adjacentes. Retourne vrai si le cout a changé, faux sinon. |
-| calculate_path | startX,startY : les coordonées du point de départ. endX,endY : les coordonées du point d'arrivée. result_path : le vecteur de points (chemin) déterminé (fournir l'adresse de la case memoire). | Calcule le chemin optimal .......... |
+| calculate_path | startX,startY : les coordonées du point de départ. endX,endY : les coordonées du point d'arrivée. result_path : le vecteur de points (chemin) déterminé (fournir l'adresse de la case memoire). | Calcule le chemin optimal .......... |

src/nav_node.cpp

@@ -18,9 +18,9 @@ Nav_node::Nav_node() : Node("nav_node"){
 
     this->robot_goal.x = -1;
     this->robot_goal.y = -1;
+    this->goal_tolerance = 0.1;
 
     // Get parameters
-    this->goal_tolerance = 0.1;
 
     this->declare_parameter("robot_id", 0);
     this->declare_parameter("pic_action_topic", "/pic_action");
@@ -70,6 +70,8 @@ void Nav_node::or_map(bool map[MAP_WIDTH][MAP_HEIGHT], bool map2[MAP_WIDTH][MAP_
     /*
     This function will do a logical OR between two maps and assign it
     to the first one
+
+	It allows user to add layer containing object like in Gimp
     */
 
     for(int x = 0; x < MAP_WIDTH; x++){
@@ -159,7 +161,7 @@ void Nav_node::obstacle_processing(Circle obstacle[3]){
     Given the three obstacles, this function will place circle on the map
     */
 
-    // Obstacle variation ?
+    // Try to see if obstacles are different
     bool variation = false;
     for (int i = 0; i < 3; i++){
         variation = true;
@@ -172,6 +174,7 @@ void Nav_node::obstacle_processing(Circle obstacle[3]){
             }
         }
     }
+
     if (!variation){
         // If there is no variation, do nothing
         return;
@@ -180,7 +183,8 @@ void Nav_node::obstacle_processing(Circle obstacle[3]){
         #ifndef WORLD_OF_SILENCE
         RCLCPP_INFO(this->get_logger(), "Obstacle variation detected");
         #endif
-
+
+		// Update the map with new obstacles
         bool new_map[MAP_WIDTH][MAP_HEIGHT];
         for (int x = 0; x < MAP_WIDTH; x++){
             for (int y = 0; y < MAP_HEIGHT; y++){
@@ -327,7 +331,7 @@ void Nav_node::goal_callback(const geometry_msgs::msg::Point msg){
 
     int result = this->nav_alg.calculate_path(this->robot_position.x, this->robot_position.y, this->robot_goal.x, this->robot_goal.y, this->path);
     if (result != 0){
-        // If the path is empty, the goal is unreachable
+        // If the result is different from 0 there is an error, please see errno description in README.md
 
         #ifndef WORLD_OF_SILENCE
         if (result == -1){

src/pstar.cpp

@@ -31,24 +31,29 @@ bool PStar::get_cost(Point p){
     Return true if the cost has changed
     */
     if (this->map[p.x][p.y] == false){
+		// (x,y) is a wall
         return false;
     }
 
     int old_cost = this->map_cost[p.x][p.y];
-    int cost = INT_MAX;
+    int cost = INT_MAX; // Set cost to +inf
+
+	// We will browse every 8 point around
     for (int dx = -1; dx <= 1; dx++){
         for (int dy = -1; dy <= 1; dy++){
             if (dx == 0 || dy == 0){
-                cost = this->map_cost[p.x + dx][p.y + dy] + 10;
+                cost = this->map_cost[p.x + dx][p.y + dy] + 10; // 10 is 10*sqrt(dx^2+dy^2)
             }
             else{
-                cost = this->map_cost[p.x + dx][p.y + dy] + 14;
+                cost = this->map_cost[p.x + dx][p.y + dy] + 14; //14 is a simplification of 10*sqrt(dx^2+dy^2)
             }
             if (cost < this->map_cost[p.x][p.y] && cost > 0){
+				// We assign the result if we found a better way to go to (x,y)
                 this->map_cost[p.x][p.y] = cost;
             }
         }
     }
+
     return old_cost != this->map_cost[p.x][p.y];
 }
 
@@ -61,10 +66,8 @@ int PStar::calculate_path(int startX, int startY, int endX, int endY, std::vecto
     start point to the end point and return it.
     */
 
-    Point tmp;
+    Point tmp; // A temporary, useless variable to store result temporarly
     bool running = true;
-    int count = 0;
-
     bool still_running = true;
 
     this->map_cost[startX][startY] = 0;
@@ -76,46 +79,46 @@ int PStar::calculate_path(int startX, int startY, int endX, int endY, std::vecto
 
     while (running){
 
-        count = 0;
         running = false;
-        for (int x = 1; x < MAP_WIDTH-1; x++){
+        for (int x = 1; x < MAP_WIDTH-1; x++){// Browse the map forward...
             for (int y = 1; y < MAP_HEIGHT-1; y++){
                 tmp.x = x;
                 tmp.y = y;
                 still_running = this->get_cost(tmp);
-                running = running || still_running;
-                if (still_running){count ++;}
+                running = running || still_running; // False only if every iteration is false
             }
         }
-        if (!running){continue;}
-        for (int x = MAP_WIDTH-2; x > 1; x--){
+        if (!running){continue;} 
+        for (int x = MAP_WIDTH-2; x > 1; x--){// ...and backward.
             for (int y = MAP_HEIGHT-2; y > 1; y--){
                 tmp.x = x;
                 tmp.y = y;
                 still_running = this->get_cost(tmp);
                 running = running || still_running;
-                if (still_running){count ++;}
             }
         }
     }
-    //std::cout << "Cost calculated" << std::endl;
+
+	// Rebuild the path starting from the end
     std::vector<Point> path;
 
     Point current;
     current.x = endX;
     current.y = endY;
     if (this->map_cost[current.x][current.y] == INT_MAX){
-        return -1;
+        return -1; // Errno -1 path not found
     }
     path.push_back(current);
     while (current.x != startX || current.y != startY){
         int best_cost = INT_MAX;
         Point best_point;
-        for (int dx = -1; dx <= 1; dx++){
+
+        for (int dx = -1; dx <= 1; dx++){ // For each neighbour find the best cost (the nearest point from start)
             for (int dy = -1; dy <= 1; dy++){
                 tmp.x = current.x + dx;
                 tmp.y = current.y + dy;
                 if (tmp.x < 0 || tmp.x >= MAP_WIDTH || tmp.y < 0 || tmp.y >= MAP_HEIGHT){
+					// If the point is out of the map, stop considering this point (or it may segfault)
                     continue;
                 }
                 if (this->map_cost[tmp.x][tmp.y] < best_cost){
@@ -124,10 +127,12 @@ int PStar::calculate_path(int startX, int startY, int endX, int endY, std::vecto
                 }
             }
         }
+		// Add the best neighbour to the path and restart searching from it
         current = best_point;
         path.push_back(current);
     }
-    // Rewrite the path in the parameter
+
+    // Rewrite the path in the vector given in parameter
     int length = path.size();
     for (int i = 0; i < length ; i++){
         result_path.push_back(path[i]);