.. _program_listing_file_src_genetic_algorithm.cpp:

Program Listing for File genetic_algorithm.cpp
==============================================

|exhale_lsh| :ref:`Return to documentation for file <file_src_genetic_algorithm.cpp>` (``src/genetic_algorithm.cpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   #include "path_planning/genetic_algorithm.hpp"
   
   #include <algorithm>
   #include <iomanip>  // TODO(vss): replace setw
   #include <iostream>
   #include <limits>
   #include <random>
   
   constexpr int random_range_max = 100;
   constexpr int inverted_probabilty_mutate = 10;
   
   void GeneticAlgorithm::SetParams(const int generations, const int popsize,
                                    const double c, const bool shorten_chromosome,
                                    const int path_length) {
     generations_ = generations;
     popsize_ = popsize;
     c_ = c;
     shorten_chromosome_ = shorten_chromosome;
     path_length_ = path_length;
   }
   
   std::tuple<bool, std::vector<Node>> GeneticAlgorithm::Plan(const Node &start,
                                                              const Node &goal) {
     int generation = 0;
     start_ = start;
     goal_ = goal;
     grid_ = original_grid_;
     motions_ = GetMotion();
   
     // Create initial chromosome
     std::vector<Node> initial_path = GenerateSimplePath();
     paths_.push_back(initial_path);
   
     // Check first path to goal
     if (CheckPath(initial_path)) {
       truepaths_.push_back(initial_path);
     }
   
     // Allow while loop to continue beyond path found to find optimum path
     while (generation <= generations_) {
       size_t paths_size = paths_.size();
       std::vector<int> f_vals(paths_size);
       std::transform(
           paths_.begin(), paths_.end(), f_vals.begin(),
           [&](const std::vector<Node> &path) { return CalculateFitness(path); });
   
       f_val = *std::min_element(f_vals.begin(), f_vals.end());
       std::vector<std::vector<Node>> new_paths_;
   
       for (int i = 0; i < paths_.size(); ++i) {
         // c provides a margin of error from best path
         if (f_vals[i] <= f_val * c_) {
           if (shorten_chromosome_) {
             const int len = std::min(paths_[i].size(), path_length_);
             new_paths_.emplace_back(std::vector<Node>(
                 paths_[i].begin(), std::next(paths_[i].begin(), len)));
           } else {
             new_paths_.push_back(paths_[i]);
           }
         }
       }
   
       if (!new_paths_.empty()) {
         std::swap(paths_, new_paths_);
       }
   
       if (paths_.size() < popsize_) {
         while (paths_.size() < popsize_) {
           if (rand() % inverted_probabilty_mutate == 0) {
             paths_.emplace_back(Mutate());
           } else {
             paths_.emplace_back(Crossover());
           }
         }
       } else {
         paths_[rand() % paths_.size()] = Mutate();
       }
   
       // TODO(vss): Consider dynamically modifying path length to move towards
       // optimality
       for (const auto &path_seen : paths_) {
         int tmp_length = std::numeric_limits<int>::max();
         if (CheckPath(path_seen)) {
           found_ = true;
           if (truepaths_.size() > (n_ * n_)) {
             truepaths_[rand() % (n_ * n_)] = path_seen;
           } else {
             truepaths_.push_back(path_seen);
           }
           if (shorten_chromosome_) {
             auto tmp = start_;
             if (path_seen.size() < tmp_length) {
               tmp_length = path_seen.size();
             }
             if (CompareCoordinates(tmp, goal_)) {
               tmp_length = 0;
             }
             for (size_t i = 0; i < path_seen.size(); i++) {
               tmp = tmp + path_seen[i];
               if (CompareCoordinates(tmp, goal_)) {
                 tmp_length = i;
                 break;
               }
             }
             path_length_ = tmp_length;
           }
         }
         // PrintChromosome(path_seen);
       }
       generation++;
     }
   
     // std::cout << "True paths: " << truepaths_.size() << '\n';
     // if(!truepaths_.empty())std::cout << "True paths: " << '\n';
     // for(int i=0;i<truepaths_.size();i++) PrintPathOfChromosome(truepaths_[i]);
   
     const auto node_path = ReturnLastPath();
     return {!node_path.empty(), node_path};
   }
   
   std::vector<Node> GeneticAlgorithm::ReturnLastPath() const {
     if (truepaths_.empty()) {
       return {};
     }
   
     std::vector<int> f_vals(truepaths_.size());
     std::transform(truepaths_.begin(), truepaths_.end(), f_vals.begin(),
                    f_vals.begin(),
                    [&](const std::vector<Node> &path, const int /* f_vals_v */) {
                      return CalculateFitness(path);
                    });
     const int best_path_index = std::distance(
         f_vals.begin(), std::min_element(f_vals.begin(), f_vals.end()));
     std::vector<Node> node_path;
     node_path.push_back(start_);
     node_path.back().pid_ = node_path.back().id_;
     Node current = start_;
     for (const auto &motion : truepaths_[best_path_index]) {
       current = current + motion;
       current.pid_ = node_path.back().id_;
       current.id_ = n_ * current.x_ + current.y_;
       node_path.push_back(current);
       if (CompareCoordinates(current, goal_)) {
         break;
       }
     }
     return node_path;
   }
   
   #ifdef CUSTOM_DEBUG_HELPER_FUNCION
   void GeneticAlgorithm::CheckIfNodesInPathAreAcceptable(
       const std::vector<Node> &path) const {
     for (const auto &motion : path) {
       bool found = false;
       for (const auto &m : motions_) {
         if (CompareCoordinates(m, motion)) {
           found = true;
           break;
         }
       }
       if (!found) {
         motion.PrintStatus();
         exit(0);
       }
     }
   }
   
   void GeneticAlgorithm::PrintChromosome(const std::vector<Node> &path) const {
     std::cout << "Chromosome: ";
     for (const auto &v : path) {
       for (size_t i = 0; i < motions_.size(); i++)
         if (CompareCoordinates(v, motions_[i])) {
           std::cout << i << " ";
         }
     }
     std::cout << "Fitness value: " << CalculateFitness(path) << '\n';
   }
   
   void GeneticAlgorithm::PrintPathOfChromosome(
       const std::vector<Node> &path) const {
     std::cout << "Path: ";
     Node tmp = start_;
     std::cout << "(" << tmp.x_ << "," << tmp.y_ << ")" << std::setw(3);
     for (auto v : path) {
       tmp = tmp + v;
       std::cout << "(" << tmp.x_ << "," << tmp.y_ << ")" << std::setw(3);
     }
     std::cout << '\n';
   }
   #endif  // CUSTOM_DEBUG_HELPER_FUNCION
   
   std::vector<Node> GeneticAlgorithm::GenerateSimplePath() const {
     int d_x = goal_.x_ - start_.x_;
     int d_y = goal_.y_ - start_.y_;
     Node dx(d_x >= 0 ? 1 : -1, 0);
     Node dy(0, d_y >= 0 ? 1 : -1);
     std::vector<Node> path(path_length_);
     auto it = path.begin();
     std::fill_n(it, std::abs(d_x), dx);
     std::advance(it, std::abs(d_x));
     std::fill_n(it, std::abs(d_y), dy);
     std::advance(it, std::abs(d_y));
     std::generate_n(it, std::distance(it, path.end()),
                     [&]() { return motions_[rand() % 4]; });
     return path;
   }
   
   int GeneticAlgorithm::CalculateFitness(const std::vector<Node> &path) const {
     int cost = 0;
     Node i = start_;
     for (const auto &p : path) {
       i = i + p;
       if (i.x_ < 0 || i.x_ >= n_ || i.y_ < 0 || i.y_ >= n_) {
         return std::numeric_limits<int>::max();
       }
       if (CompareCoordinates(i, goal_)) {
         break;
       }
       if (grid_[i.x_][i.y_] == 1) {
         cost += n_ * abs(goal_.x_ - i.x_) + n_ * abs(goal_.y_ - i.y_);
       } else {
         // Can add a scaling factor here
         cost += abs(goal_.x_ - i.x_) + abs(goal_.y_ - i.y_);
       }
     }
     return cost;
   }
   
   std::vector<Node> GeneticAlgorithm::GenerateRandomPath() const {
     std::random_device rd;
     std::mt19937 eng(rd());
     std::vector<Node> path(path_length_);
     if (path_length_ > 0) {
       std::uniform_int_distribution<int> distr(
           0, motions_.size() - 1);  // define the range
       std::generate_n(path.begin(), path_length_,
                       [&]() { return motions_[distr(eng)]; });
     }
     return path;
   }
   
   std::vector<Node> GeneticAlgorithm::Mutate() const {
     std::random_device rd;
     std::mt19937 eng(rd());
     std::vector<Node> path;
     if (truepaths_.size() > 3 && rand() % 4 == 0) {
       path = truepaths_[rand() % truepaths_.size()];
     } else {
       path = GenerateRandomPath();
     }
     if (path.empty()) {
       return path;
     }
     int index_1 = static_cast<int>(rand() % path.size());
     int index_2 = static_cast<int>(rand() % path.size());
     if (index_2 < index_1) {
       std::swap(index_1, index_2);
     }
     std::shuffle(path.begin() + index_1, path.begin() + index_2, eng);
     return path;
   }
   
   std::vector<Node> GeneticAlgorithm::Crossover() const {
     int p1 = static_cast<int>(rand() % paths_.size());
     int p2 = static_cast<int>(rand() % paths_.size());
   
     const size_t len =
         std::max(std::max(paths_[p1].size(), paths_[p2].size()), path_length_);
     std::vector<Node> child;
     child.reserve(len);
   
     Node current = start_;
   
     int index = 0;
     int attempt_count = 0;
   
     while (index < len) {
       int random_int = rand() % random_range_max;
       Node motion;
       if (random_int < random_range_max / 4 && paths_[p1].size() > index) {
         motion = paths_[p1][index];
       } else if (random_int < random_range_max / 2 && paths_[p2].size() > index) {
         motion = paths_[p2][index];
       } else {
         motion = motions_[rand() % motions_.size()];
       }
       // Prevents the new chromosome from going beyond the grid
       // Added as a very large percentage (majority) of these are out of bounds,
       // and a waste of resources Chromosomes that contain paths travelling over
       // obstacles are still allowed after a point
       if (auto new_node = current + motion; !checkOutsideBoundary(new_node, n_)) {
         if (grid_[new_node.x_][new_node.y_] != 1 || attempt_count > n_ * n_) {
           current = new_node;
           child.push_back(motion);
           ++index;
         }
       }
       ++attempt_count;
     }
     return child;
   }
   
   // NOTE: Consider storing the point where an obstacle is encountereed and forcig
   // that gene/motion to randomly mutate for a quicker convergence to a solution
   // while maintaining randomness
   bool GeneticAlgorithm::CheckPath(const std::vector<Node> &path) const {
     Node current = start_;
     if (CompareCoordinates(current, goal_)) {
       return true;
     }
     for (const auto &node : path) {
       current = current + node;
       if (CompareCoordinates(current, goal_)) {
         return true;
       }
       if (checkOutsideBoundary(current, n_)) {
         return false;
       }
       if (grid_[current.x_][current.y_] == 1) {
         return false;
       }
     }
     return CompareCoordinates(current, goal_);
   }
   
   #ifdef BUILD_INDIVIDUAL
   int main() {
     constexpr int n = 11;
     std::vector<std::vector<int>> grid(n, std::vector<int>(n, 0));
     MakeGrid(grid);
   
     std::random_device rd;   // obtain a random number from hardware
     std::mt19937 eng(rd());  // seed the generator
     std::uniform_int_distribution<int> distr(0, n - 1);  // define the range
   
     Node start(distr(eng), distr(eng), 0, 0, 0, 0);
     Node goal(distr(eng), distr(eng), 0, 0, 0, 0);
   
     start.id_ = start.x_ * n + start.y_;
     start.pid_ = start.x_ * n + start.y_;
     goal.id_ = goal.x_ * n + goal.y_;
     start.h_cost_ = abs(start.x_ - goal.x_) + abs(start.y_ - goal.y_);
   
     // Make sure start and goal are not obstacles and their ids are correctly
     // assigned.
     grid[start.x_][start.y_] = 0;
     grid[goal.x_][goal.y_] = 0;
   
     start.PrintStatus();
     goal.PrintStatus();
   
     PrintGrid(grid);
   
     constexpr int generations = 10000;
     constexpr int popsize = 30;
     constexpr double c = 1.05;
     constexpr bool shorten_chromosome = true;
     constexpr int path_length_x_factor = 4;
   
     GeneticAlgorithm new_genetic_algorithm(grid);
     new_genetic_algorithm.SetParams(
         generations, popsize, c, shorten_chromosome,
         static_cast<int>(path_length_x_factor * start.h_cost_));
     const auto [path_found, path_vector] =
         new_genetic_algorithm.Plan(start, goal);
     PrintPathInOrder(path_vector, start, goal, grid);
   }
   #endif  // BUILD_INDIVIDUAL