diff --git a/README.md b/README.md index c0c9bf5..4ac35b6 100644 --- a/README.md +++ b/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 .......... | \ No newline at end of file +| 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 .......... | diff --git a/src/nav_node.cpp b/src/nav_node.cpp index 3625539..2622f32 100644 --- a/src/nav_node.cpp +++ b/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){ diff --git a/src/pstar.cpp b/src/pstar.cpp index 8c58ebf..dad07a7 100644 --- a/src/pstar.cpp +++ b/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 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]);