Skip to content

Commit

Permalink
fix bug ai-winter#39 and update GA
Browse files Browse the repository at this point in the history
  • Loading branch information
@ai-winter
ai-winter committed Dec 21, 2023
1 parent 9ce090b commit bf648e1
Show file tree
Hide file tree
Showing 9 changed files with 522 additions and 674 deletions.
4 changes: 3 additions & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -312,7 +312,7 @@ Explanation:
| Planner | Version | Animation |
|:-------:|:-------:|:---------:|
| **ACO** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/evolutionary_planner/src/aco.cpp) | ![aco_ros.gif](assets/aco_ros.gif) |
| **GA** | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) |
| **GA** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/evolutionary_planner/src/ga.cpp) | ![ga_ros.gif](assets/ga_ros.gif) |
| **PSO** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/evolutionary_planner/src/pso.cpp) | ![pso_ros.gif](assets/pso_ros.gif) |
| **ABC** | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) |
Expand All @@ -336,8 +336,10 @@ Explanation:
### Evolutionary-based Planning
* ACO: [Ant Colony Optimization: A New Meta-Heuristic](http://www.cs.yale.edu/homes/lans/readings/routing/dorigo-ants-1999.pdf).
* GA: [Adaptation in Natural and Artificial Systems](https://ieeexplore.ieee.org/book/6267401)
* PSO: [Particle Swarm Optimization](https://ieeexplore.ieee.org/document/488968)
### Local Planning
* DWA: [The Dynamic Window Approach to Collision Avoidance](https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf).
Expand Down
Binary file modified assets/ga_ros.gif
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
282 changes: 110 additions & 172 deletions src/planner/global_planner/evolutionary_planner/include/ga.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,11 @@
*
* @file: ga.h
* @breif: Contains the Genetic Algorithm(GA) planner class
* @author: Jing Zongxin
* @update: 2023-12-15
* @version: 1.0
* @author: Jing Zongxin, Yang Haodong
* @update: 2023-12-21
* @version: 1.1
*
* Copyright (c) 2023，Jing Zongxin
* Copyright (c) 2023， Jing Zongxin, Yang Haodong
* All rights reserved.
* --------------------------------------------------------
*
Expand All @@ -19,184 +19,122 @@
#include <mutex>
#include <vector>
#include "global_planner.h"
#include "trajectoryGeneration.h"
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include "bspline_curve.h"

#define GEN_MODE_RANDOM 1
#define GEN_MODE_CIRCLE 2

using PositionSequence = std::vector<std::vector<std::pair<int, int>>>;

namespace global_planner
{
struct Genets
struct Genets
{
std::vector<std::pair<int, int>> position; // genets position
double fitness; // genets fitness
std::vector<std::pair<int, int>> best_pos; // Personal best position in iteration
double best_fitness; // Personal best fitness in iteration

Genets() = default;

Genets(const std::vector<std::pair<int, int>>& initial_position, double initial_fitness)
: position(initial_position), fitness(initial_fitness), best_pos(initial_position), best_fitness(initial_fitness)
{
std::vector<std::pair<int, int>> position; // genets position
double fitness; // genets fitness
std::vector<std::pair<int, int>> personal_best_pos; // Personal best position in iteration
double personal_best_fitness; // Personal best fitness in iteration

Genets() = default;

Genets(const std::vector<std::pair<int, int>>& initial_position,
double initial_fitness)
: position(initial_position),
fitness(initial_fitness),
personal_best_pos(initial_position),
personal_best_fitness(initial_fitness)
{
}
};
}
};

/**
* @brief Class for objects that plan using the GA algorithm
*/

class GA : public GlobalPlanner
{
public:
/**
* @brief Class for objects that plan using the GA algorithm
* @brief Construct a new GA object
* @param nx pixel number in costmap x direction
* @param ny pixel number in costmap y direction
* @param resolution costmap resolution
* @param n_genets number of genets
* @param n_inherited number of inherited genets
* @param chromLength number of position points contained in each genets
* @param p_select selection probability
* @param p_crs crossover probability
* @param p_mut mutation probability
* @param max_speed The maximum movement speed of genets
* @param init_mode Set the generation mode for the initial position points of the genets swarm
* @param pub_genets Boolean flag to publish genets.
* @param max_iter maximum iterations
*/
GA(int nx, int ny, double resolution, int n_genets, int n_inherited, int chromLength, double p_select, double p_crs,
double p_mut, int max_speed, int init_mode, bool pub_genets, int max_iter);
~GA();

class GA : public GlobalPlanner
{
public:
/**
* @brief Construct a new GA object
* @param nx pixel number in costmap x direction
* @param ny pixel number in costmap y direction
* @param resolution costmap resolution
* @param origin_x origin coordinate in the x direction.
* @param origin_y origin coordinate in the y direction.
* @param n_genets number of genets
* @param n_inherited number of inherited genets
* @param chromLength number of position points contained in each genets
* @param p_select selection probability
* @param p_crs crossover probability
* @param p_mut mutation probability
* @param max_speed The maximum movement speed of genets
* @param initposmode Set the generation mode for the initial position points of the genets swarm
* @param pub_genets Boolean flag to publish genets.
* @param max_iter maximum iterations
*/
GA(int nx, int ny, double resolution, double origin_x, double origin_y, int n_genets,int n_inherited, int chromLength , double p_select, double p_crs, double p_mut, int max_speed,int initposmode ,bool pub_genets,int max_iter);
~GA();

/**
* @brief GA implementation
* @param global_costmap global costmap
* @param start start node
* @param goal goal node
* @param path optimal path consists of Node
* @param expand containing the node been search during the process(unused)
* @return true if path found, else false
*/
bool plan(const unsigned char* global_costmap, const Node& start, const Node& goal, std::vector<Node>& path,
std::vector<Node>& expand);

/**
* @brief Generate n genets with pointNum_ positions each within the map range
* @param initialPositions The initial position sequence of genets swarm
* @param start_d starting point
* @param goal_d Target point
*/
void generateRandomInitialPositions(PositionSequence &initialPositions,const std::pair<double, double> start_d,const std::pair<double, double> goal_d);

/**
* @brief Generate an initial position point sequence within a circular area, located within the map range
* @param initialPositions The initial position sequence of genets swarm
* @param start_d starting point
* @param goal_d Target point
*/
void generateCircularInitialPositions(PositionSequence &initialPositions,const std::pair<double, double> start_d,const std::pair<double, double> goal_d);

/**
* @brief Calculate Obstacle avoidance cost
* @param global_costmap global costmap
* @param ga_path Path to perform collision detection
* @return The collision cost of the path
*/
double ObstacleCost(const unsigned char* global_costmap,const std::vector<std::pair<double, double>>& ga_path);


/**
* @brief Perform selection using roulette wheel method.
* @param population The population of Genets.
* @param p_select The selection probability.
* @param selectedPopulation The selected individuals will be stored in this vector.
*/
void ga_select(const std::vector<Genets>& population, double p_select, std::vector<Genets>& selectedPopulation);

/**
* @brief Select individuals to be retained based on their fitness level
* @param population The population of Genets.
* @param p_select The selection probability.
* @param selectedPopulation The selected individuals will be stored in this vector.
*/
void ga_select_optimal(const std::vector<Genets>& population, double p_select, std::vector<Genets>& selectedPopulation);

/**
* @brief Genets update optimization iteration
* @param Genets_p individuals selected for retention
* @param Genets_c descendants of selected individuals to be retained
* @param Best_genets Global optimal genets
* @param start_d starting point
* @param goal_d Target point
* @param index_i genets ID
* @param global_costmap global costmap
* @param gen randomizer
*/
void optimizeGenets(const Genets& Genets_p, Genets& Genets_c, Genets& Best_genets,
const unsigned char* global_costmap, const std::pair<double, double>& start_d,
const std::pair<double, double>& goal_d,const int& index_i,std::mt19937& gen);

/**
* @brief Clamps a value within a specified range.
* @tparam T The type of the values to be clamped.
* @param value The value to be clamped.
* @param low The lower bound of the range.
* @param high The upper bound of the range.
* @return const T& The clamped value within the specified range.
*/
template <typename T>
const T& clamp(const T& value, const T& low, const T& high) {return std::max(low, std::min(value, high));}

/**
* @brief Custom comparison function for ascending order.
* @param a The first element to be compared.
* @param b The second element to be compared.
* @return bool True if 'a' is less than 'b', indicating ascending order; otherwise, false.
*/
static bool ascendingOrder(int a, int b) { return a < b;}

/**
* @brief Custom comparison function for descending order.
* @param a The first element to be compared.
* @param b The second element to be compared.
* @return bool True if 'a' is greater than 'b', indicating descending order; otherwise, false.
*/
static bool descendingOrder(int a, int b){ return a > b;}


/**
* @brief Publishes genets markers based on given positions and index.
* @param positions Vector of pairs representing positions (x, y) of genets.
* @param index Index identifier for the genets.
*/
void publishGenetsMarkers(const std::vector<std::pair<int, int>>& positions, const int& index);


protected:
double origin_x_,origin_y_; // origin coordinate in the x、y direction.
bool pub_genets_; // boolean flag to publish genets.
int max_iter_; // maximum iterations
int n_genets_; // number of genets
int n_inherited_; // number of inherited genets
int chromLength_; // number of position points contained in each genets
bool select_mode_; // Selection process settings
double p_select_, p_crs_, p_mut_; // selection probability crossover probability mutation probability
int max_speed_; // The maximum velocity of genets motion
int initposmode_; // Set the generation mode for the initial position points of the genets swarm

private:
ros::Publisher genets_pub_; //The publisher of real-time genets visualization
int GlobalBest_genets_; //The ID of the globally optimal genets
std::mutex genets_lock_; //thread lock
std::vector<Genets> inherited_genets_; //inherited genets
trajectoryGeneration path_generation; //Path generation
};
/**
* @brief GA implementation
* @param global_costmap global costmap
* @param start start node
* @param goal goal node
* @param path optimal path consists of Node
* @param expand containing the node been search during the process
* @return true if path found, else false
*/
bool plan(const unsigned char* global_costmap, const Node& start, const Node& goal, std::vector<Node>& path,
std::vector<Node>& expand);

/**
* @brief Generate n particles with pointNum_ positions each within the map range
* @param initial_positions The initial position sequence of particle swarm
* @param start start node
* @param goal goal node
* @param gen_mode generation mode
*/
void initializePositions(PositionSequence& initial_positions, const Node& start, const Node& goal,
int gen_mode = GEN_MODE_CIRCLE);

/**
* @brief Calculate the value of fitness function
* @param position the control points calculated by PSO
* @return fitness the value of fitness function
*/
double calFitnessValue(std::vector<std::pair<int, int>> position);

/**
* @brief Perform selection.
* @param population The population of Genets.
* @param selected_population The selected individuals will be stored in this vector.
*/
void selection(const std::vector<Genets>& population, std::vector<Genets>& selected_population);

/**
* @brief Genets update optimization iteration
* @param genets_p individuals selected for retention
* @param genets_c descendants of selected individuals to be retained
* @param best_genets Global optimal genets
* @param i genets ID
* @param gen randomizer
* @param expand containing the node been search during the process
*/
void optimizeGenets(const Genets& genets_p, Genets& genets_c, Genets& best_genets, const int& i, std::mt19937& gen,
std::vector<Node>& expand);

protected:
int max_iter_; // maximum iterations
int n_genets_; // number of genets
int n_inherited_; // number of inherited genets
int point_num_; // number of position points contained in each genets
double p_select_, p_crs_, p_mut_; // selection probability crossover probability mutation probability
int max_speed_; // The maximum velocity of genets motion
int init_mode_; // Set the generation mode for the initial position points of the genets swarm
std::pair<double, double> start_, goal_; // paired start and goal point for path smoothing
const unsigned char* costmap_;

private:
int best_genet_idx_; // The ID of the globally optimal genets
std::mutex genets_lock_; // thread lock
std::vector<Genets> inherited_genets_; // inherited genets
trajectory_generation::BSpline bspline_gen_; // Path generation
};

} // namespace global_planner
#endif
Loading

0 comments on commit bf648e1

Please sign in to comment.