Skip to content

Commit

Permalink
update: ACO(multi-threads)
Browse files Browse the repository at this point in the history
  • Loading branch information
@ai-winter
ai-winter committed Jul 17, 2023
1 parent 66628d2 commit 7f0b83f
Show file tree
Hide file tree
Showing 9 changed files with 187 additions and 161 deletions.
24 changes: 12 additions & 12 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -171,17 +171,17 @@ Explanation:

| Planner | Version | Animation |
| :--------------: | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------: | :--------------------------------------------------: |
| **GBFS** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/graph_planner/src/a_star.cpp) | ![gbfs_ros.gif](assets/gbfs_ros.gif) |
| **Dijkstra** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/graph_planner/src/a_star.cpp) | ![dijkstra_ros.gif](assets/dijkstra_ros.gif) |
| **A\*** | [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/graph_planner/src/a_star.cpp) | ![a_star_ros.gif](assets/a_star_ros.gif) |
| **JPS** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/graph_planner/src/jump_point_search.cpp) | ![jps_ros.gif](assets/jps_ros.gif) |
| **D\*** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/graph_planner/src/d_star.cpp)) | ![d_star_ros.gif](assets/d_star_ros.gif) |
| **LPA\*** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/graph_planner/src/lpa_star.cpp)) | ![lpa_star_ros.gif](assets/lpa_star_ros.gif) |
| **D\* Lite** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/graph_planner/src/d_star_lite.cpp)) | ![d_star_lite_ros.gif](assets/d_star_lite_ros.gif) |
| **RRT** | [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/sample_planner/src/rrt.cpp) | ![rrt_ros.gif](assets/rrt_ros.gif) |
| **RRT\*** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/sample_planner/src/rrt_star.cpp) | ![rrt_star_ros.gif](assets/rrt_star_ros.gif) |
| **Informed RRT** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/sample_planner/src/informed_rrt.cpp) | ![informed_rrt_ros.gif](assets/informed_rrt_ros.gif) |
| **RRT-Connect** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/sample_planner/src/rrt_connect.cpp) | ![rrt_connect_ros.gif](assets/rrt_connect_ros.gif) |
| **GBFS** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/a_star.cpp) | ![gbfs_ros.gif](assets/gbfs_ros.gif) |
| **Dijkstra** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/a_star.cpp) | ![dijkstra_ros.gif](assets/dijkstra_ros.gif) |
| **A\*** | [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/a_star.cpp) | ![a_star_ros.gif](assets/a_star_ros.gif) |
| **JPS** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/jump_point_search.cpp) | ![jps_ros.gif](assets/jps_ros.gif) |
| **D\*** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/d_star.cpp)) | ![d_star_ros.gif](assets/d_star_ros.gif) |
| **LPA\*** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/lpa_star.cpp)) | ![lpa_star_ros.gif](assets/lpa_star_ros.gif) |
| **D\* Lite** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/d_star_lite.cpp)) | ![d_star_lite_ros.gif](assets/d_star_lite_ros.gif) |
| **RRT** | [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/sample_planner/src/rrt.cpp) | ![rrt_ros.gif](assets/rrt_ros.gif) |
| **RRT\*** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/sample_planner/src/rrt_star.cpp) | ![rrt_star_ros.gif](assets/rrt_star_ros.gif) |
| **Informed RRT** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/sample_planner/src/informed_rrt.cpp) | ![informed_rrt_ros.gif](assets/informed_rrt_ros.gif) |
| **RRT-Connect** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/sample_planner/src/rrt_connect.cpp) | ![rrt_connect_ros.gif](assets/rrt_connect_ros.gif) |

### Local Planner

Expand All @@ -198,7 +198,7 @@ Explanation:

| Planner | Version | Animation |
| :-----: | :------------------------------------------------------: | :-----------------------------------------------------: |
| **ACO** | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) |
| **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) |
| **PSO** | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) |
| **ABC** | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) |
Expand Down
Binary file added assets/aco_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.
56 changes: 36 additions & 20 deletions src/planner/global_planner/evolutionary_planner/include/aco.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,22 +14,27 @@
#ifndef ACO_H
#define ACO_H

#include <thread>
#include <mutex>

#include "global_planner.h"

namespace aco_planner
{

struct Ant {
struct Ant
{
/**
* @brief Constructor to create a new ant
* @param cur_node Current node for ant
*/
Ant(const Node& cur_node = Node()) : cur_node_(cur_node) {}

Node cur_node_; // current node for ant
bool found_goal_ = false; // whether found goal or not
int steps_ = 0; // iteration steps
// history path nodes
Ant(const Node& cur_node = Node()) : cur_node_(cur_node)
{
}

Node cur_node_; // current node for ant
bool found_goal_ = false; // whether found goal or not
int steps_ = 0; // iteration steps
// history path nodes
std::unordered_set<Node, NodeIdAsHash, compare_coordinates> path_;
};

Expand All @@ -44,14 +49,15 @@ class ACO : public global_planner::GlobalPlanner
* @param nx pixel number in costmap x direction
* @param ny pixel number in costmap y direction
* @param resolution costmap resolution
* @param n_ants number of ants
* @param alpha pheromone weight coefficient
* @param beta heuristic factor weight coefficient
* @param rho evaporation coefficient
* @param Q pheromone gain
* @param max_iter maximum iterations
* @param n_ants number of ants
* @param alpha pheromone weight coefficient
* @param beta heuristic factor weight coefficient
* @param rho evaporation coefficient
* @param Q pheromone gain
* @param max_iter maximum iterations
*/
ACO(int nx, int ny, double resolution, int n_ants, double alpha, double beta, double rho, double Q, int max_iter);
~ACO();

/**
* @brief ACO implementation
Expand All @@ -65,13 +71,23 @@ class ACO : public global_planner::GlobalPlanner
bool plan(const unsigned char* gloal_costmap, const Node& start, const Node& goal, std::vector<Node>& path,
std::vector<Node>& expand);

void antSearch(const unsigned char* gloal_costmap, const Node& start, const Node& goal);

protected:
int n_ants_; // number of ants
double alpha_, beta_; // pheromone and heuristic factor weight coefficient
double rho_; // evaporation coefficient
double Q_; // pheromone gain
int max_iter_; // maximum iterations
int n_ants_; // number of ants
double alpha_, beta_; // pheromone and heuristic factor weight coefficient
double rho_; // evaporation coefficient
double Q_; // pheromone gain
int max_iter_; // maximum iterations

// best path and its length
int best_path_length_;
std::unordered_set<Node, NodeIdAsHash, compare_coordinates> best_path_;
std::vector<Node> motion_; // all possible motions
double* pheromone_edges_; // pheromone matrix

private:
std::mutex lock_;
};
} // namespace a_star_planner
} // namespace aco_planner
#endif
227 changes: 112 additions & 115 deletions src/planner/global_planner/evolutionary_planner/src/aco.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,13 @@ ACO::ACO(int nx, int ny, double resolution, int n_ants, double alpha, double bet
, Q_(Q)
, max_iter_(max_iter)
{
motion_ = getMotion();
pheromone_edges_ = new double[static_cast<int>(nx_ * ny_ * motion_.size())];
}

ACO::~ACO()
{
delete[] pheromone_edges_;
}

/**
Expand All @@ -54,140 +61,130 @@ ACO::ACO(int nx, int ny, double resolution, int n_ants, double alpha, double bet
bool ACO::plan(const unsigned char* gloal_costmap, const Node& start, const Node& goal, std::vector<Node>& path,
std::vector<Node>& expand)
{
// best path
int best_path_length = std::numeric_limits<int>::max();
std::unordered_set<Node, NodeIdAsHash, compare_coordinates> best_path;

// get all possible motions
const std::vector<Node> motion = getMotion();

// pheromone initialization
double* pheromone_edges = new double[static_cast<int>(nx_ * ny_ * motion.size())];
for (size_t k = 0; k < nx_ * ny_ * motion.size(); k++)
pheromone_edges[k] = 1.0;
for (size_t k = 0; k < nx_ * ny_ * motion_.size(); k++)
pheromone_edges_[k] = 1.0;

// heuristically set max steps
int max_steps = nx_ * ny_ / 2;

// main loop
best_path_length_ = std::numeric_limits<int>::max();
for (size_t i = 0; i < max_iter_; i++)
{
std::vector<Ant> ants_list = std::vector<Ant>(n_ants_);
std::vector<std::thread> ants_list = std::vector<std::thread>(n_ants_);
for (size_t j = 0; j < n_ants_; j++)
{
Ant ant(start);
while ((ant.cur_node_ != goal) && ant.steps_ < max_steps)
{
ant.path_.insert(ant.cur_node_);

// candidate
float prob_sum = 0.0;
std::vector<Node> next_positions;
std::vector<double> next_probabilities;

for (size_t z = 0; z < motion.size(); z++)
{
Node node_n = ant.cur_node_ + motion[z];
node_n.id_ = grid2Index(node_n.x_, node_n.y_);

// next node hit the boundary or obstacle
if ((node_n.id_ < 0) || (node_n.id_ >= ns_) || (gloal_costmap[node_n.id_] >= lethal_cost_ * factor_))
continue;

// current node exists in history path
if (ant.path_.find(node_n) != ant.path_.end())
continue;

node_n.pid_ = ant.cur_node_.id_;

// goal found
if (node_n == goal)
{
ant.path_.insert(node_n);
ant.found_goal_ = true;
break;
}
next_positions.push_back(node_n);
double prob_new =
std::pow(pheromone_edges[motion.size() * ant.cur_node_.x_ + nx_ * ant.cur_node_.y_ + z], alpha_) *
std::pow(1.0 / std::sqrt(std::pow((node_n.x_ - goal.x_), 2) + std::pow((node_n.y_ - goal.y_), 2)), beta_);
next_probabilities.push_back(prob_new);
prob_sum += prob_new;
}
// Ant in a cul-de-sac or no next node (ie start surrounded by obstacles)
if (prob_sum == 0 || ant.found_goal_)
break;

// roulette selection
std::for_each(next_probabilities.begin(), next_probabilities.end(), [&](double& p) { p /= prob_sum; });
std::random_device device;
std::mt19937 engine(device());
std::discrete_distribution<> dist(next_probabilities.begin(), next_probabilities.end());
ant.cur_node_ = next_positions[dist(engine)];
ant.steps_ += 1;
}
ants_list[j] = ant;
}
ants_list[j] = std::thread(&ACO::antSearch, this, gloal_costmap, start, goal);
for (size_t j = 0; j < n_ants_; j++)
ants_list[j].join();

// pheromone deterioration
for (size_t k = 0; k < nx_ * ny_ * motion.size(); k++)
pheromone_edges[k] *= (1 - rho_);
for (size_t k = 0; k < nx_ * ny_ * motion_.size(); k++)
pheromone_edges_[k] *= (1 - rho_);
}

// pheromone update based on successful ants
int bpl = std::numeric_limits<int>::max();
std::unordered_set<Node, NodeIdAsHash, compare_coordinates> bp;
for (const Ant& ant : ants_list)
if (best_path_.size() > 0)
{
path = _convertClosedListToPath(best_path_, start, goal);
return true;
}
return false;
}

void ACO::antSearch(const unsigned char* gloal_costmap, const Node& start, const Node& goal)
{
int max_steps = nx_ * ny_ / 2;
Ant ant(start);

while ((!ant.found_goal_) && (ant.cur_node_ != goal) && (ant.steps_ < max_steps))
{
ant.path_.insert(ant.cur_node_);

// candidate
float prob_sum = 0.0;
std::vector<Node> next_positions;
std::vector<double> next_probabilities;

for (size_t z = 0; z < motion_.size(); z++)
{
if (ant.found_goal_)
Node node_n = ant.cur_node_ + motion_[z];
node_n.id_ = grid2Index(node_n.x_, node_n.y_);

// next node hit the boundary or obstacle
if ((node_n.id_ < 0) || (node_n.id_ >= ns_) || (gloal_costmap[node_n.id_] >= lethal_cost_ * factor_))
continue;

// current node exists in history path
if (ant.path_.find(node_n) != ant.path_.end())
continue;

node_n.pid_ = ant.cur_node_.id_;

// goal found
if (node_n == goal)
{
if (static_cast<int>(ant.path_.size()) < bpl)
{
// save best path yet in this iteration
bpl = ant.path_.size();
bp = ant.path_;
}

// reward here, increased pheromone
double c = Q_ / static_cast<double>(ant.path_.size() - 1);
for (const auto node : ant.path_)
{
// TODO:
int px, py, z;
index2Grid(node.pid_, px, py);
if (node.x_ - px == 0 && node.y_ - py == 1)
z = 0;
else if (node.x_ - px == 1 && node.y_ - py == 0)
z = 1;
else if (node.x_ - px == 0 && node.y_ - py == -1)
z = 2;
else if (node.x_ - px == -1 && node.y_ - py == 0)
z = 3;
else if (node.x_ - px == 1 && node.y_ - py == 1)
z = 4;
else if (node.x_ - px == 1 && node.y_ - py == -1)
z = 5;
else if (node.x_ - px == -1 && node.y_ - py == 1)
z = 6;
else
z = 7;
pheromone_edges[motion.size() * px + nx_ * py + z] += c;
}
ant.path_.insert(node_n);
ant.found_goal_ = true;
break;
}
next_positions.push_back(node_n);
double prob_new =
std::pow(pheromone_edges_[motion_.size() * ant.cur_node_.x_ + nx_ * ant.cur_node_.y_ + z], alpha_) *
std::pow(1.0 / std::sqrt(std::pow((node_n.x_ - goal.x_), 2) + std::pow((node_n.y_ - goal.y_), 2)), beta_);
next_probabilities.push_back(prob_new);
prob_sum += prob_new;
}

// update best path
if (bpl <= best_path_length)
// Ant in a cul-de-sac or no next node (ie start surrounded by obstacles)
if (prob_sum == 0 || ant.found_goal_)
break;

// roulette selection
std::for_each(next_probabilities.begin(), next_probabilities.end(), [&](double& p) { p /= prob_sum; });
std::random_device device;
std::mt19937 engine(device());
std::discrete_distribution<> dist(next_probabilities.begin(), next_probabilities.end());
ant.cur_node_ = next_positions[dist(engine)];
ant.steps_ += 1;
}

// pheromone update based on successful ants
lock_.lock();
if (ant.found_goal_)
{
if (static_cast<int>(ant.path_.size()) < best_path_length_)
{
best_path_length = bpl;
best_path = bp;
// save best path yet in this iteration
best_path_length_ = ant.path_.size();
best_path_ = ant.path_;
}
}

if (best_path.size() > 0)
{
path = _convertClosedListToPath(best_path, start, goal);
return true;
// reward here, increased pheromone
double c = Q_ / static_cast<double>(ant.path_.size() - 1);
for (const auto node : ant.path_)
{
// TODO:
int px, py, z;
index2Grid(node.pid_, px, py);
if (node.x_ - px == 0 && node.y_ - py == 1)
z = 0;
else if (node.x_ - px == 1 && node.y_ - py == 0)
z = 1;
else if (node.x_ - px == 0 && node.y_ - py == -1)
z = 2;
else if (node.x_ - px == -1 && node.y_ - py == 0)
z = 3;
else if (node.x_ - px == 1 && node.y_ - py == 1)
z = 4;
else if (node.x_ - px == 1 && node.y_ - py == -1)
z = 5;
else if (node.x_ - px == -1 && node.y_ - py == 1)
z = 6;
else
z = 7;
pheromone_edges_[motion_.size() * px + nx_ * py + z] += c;
}
}
return false;
lock_.unlock();
}

} // namespace aco_planner
Loading

0 comments on commit 7f0b83f

Please sign in to comment.