Skip to content

Commit

Permalink
update: improve the nodes class
Browse files Browse the repository at this point in the history
  • Loading branch information
ai-winter committed Jun 10, 2024
1 parent ba2f48c commit 033a80c
Show file tree
Hide file tree
Showing 26 changed files with 498 additions and 403 deletions.
20 changes: 10 additions & 10 deletions src/core/global_planner/evolutionary_planner/src/aco.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,8 +61,8 @@ ACO::~ACO()
*/
bool ACO::plan(const Node& start, const Node& goal, std::vector<Node>& path, std::vector<Node>& expand)
{
start_ = std::pair<double, double>(static_cast<double>(start.x_), static_cast<double>(start.y_));
goal_ = std::pair<double, double>(static_cast<double>(goal.x_), static_cast<double>(goal.y_));
start_ = std::pair<double, double>(static_cast<double>(start.x()), static_cast<double>(start.y()));
goal_ = std::pair<double, double>(static_cast<double>(goal.x()), static_cast<double>(goal.y()));
expand.clear();
for (size_t i = 0; i < map_size_; i++)
pheromone_mat_[i] = 1.0;
Expand Down Expand Up @@ -98,10 +98,10 @@ bool ACO::plan(const Node& start, const Node& goal, std::vector<Node>& path, std

// Generating Paths from Optimal Particles
std::vector<std::pair<double, double>> points, b_path;
points.emplace_back(static_cast<double>(start.x_), static_cast<double>(start.y_));
points.emplace_back(static_cast<double>(start.x()), static_cast<double>(start.y()));
for (const auto& pos : best_ant.position)
points.emplace_back(static_cast<double>(pos.first), static_cast<double>(pos.second));
points.emplace_back(static_cast<double>(goal.x_), static_cast<double>(goal.y_));
points.emplace_back(static_cast<double>(goal.x()), static_cast<double>(goal.y()));
points.erase(std::unique(std::begin(points), std::end(points)), std::end(points));

bspline_gen_.run(points, b_path);
Expand All @@ -122,7 +122,7 @@ bool ACO::plan(const Node& start, const Node& goal, std::vector<Node>& path, std
int y = static_cast<int>(b_path[p].second);

// Check if the current point is different from the last point
if (x != path.back().x_ || y != path.back().y_)
if (x != path.back().x() || y != path.back().y())
path.emplace_back(x, y, 0.0, 0.0, p, 0);
}
}
Expand Down Expand Up @@ -153,17 +153,17 @@ void ACO::initializePositions(PositionSequence& initial_positions, const Node& s
int point_id, pos_id;

// Calculate sequence direction
bool x_order = (goal.x_ > start.x_);
bool y_order = (goal.y_ > start.y_);
bool x_order = (goal.x() > start.x());
bool y_order = (goal.y() > start.y());

// circle generation
int center_x, center_y;
double radius;
if (gen_mode == GEN_MODE_CIRCLE)
{
// Calculate the center and the radius of the circle (midpoint between start and goal)
center_x = (start.x_ + goal.x_) / 2;
center_y = (start.y_ + goal.y_) / 2;
center_x = (start.x() + goal.x()) / 2;
center_y = (start.y() + goal.y()) / 2;
radius = helper::dist(start, goal) / 2.0 < 5 ? 5 : helper::dist(start, goal) / 2.0;
}

Expand Down Expand Up @@ -210,7 +210,7 @@ void ACO::initializePositions(PositionSequence& initial_positions, const Node& s
point_id++;
visited.insert(pos_id);
double prob_new = std::pow(pheromone_mat_[pos_id], alpha_) *
std::pow(1.0 / hypot(temp_x[point_id] - goal.x_, temp_y[point_id] - goal.y_), beta_);
std::pow(1.0 / hypot(temp_x[point_id] - goal.x(), temp_y[point_id] - goal.y()), beta_);
probabilities.push_back(prob_new);
prob_sum += prob_new;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -325,7 +325,7 @@ bool EvolutionaryPlanner::_getPlanFromPath(std::vector<Node>& path, std::vector<
for (int i = path.size() - 1; i >= 0; i--)
{
double wx, wy;
g_planner_->map2World((double)path[i].x_, (double)path[i].y_, wx, wy);
g_planner_->map2World((double)path[i].x(), (double)path[i].y(), wx, wy);

// coding as message type
geometry_msgs::PoseStamped pose;
Expand Down Expand Up @@ -375,8 +375,8 @@ void EvolutionaryPlanner::_publishExpand(std::vector<Node>& expand)
for (const auto& node : expand)
{
geometry_msgs::Point p;
p.x = origin_x + node.x_ * resolution;
p.y = origin_y + node.y_ * resolution;
p.x = origin_x + node.x() * resolution;
p.y = origin_y + node.y() * resolution;
p.z = 0.0;
marker.points.push_back(p);
}
Expand Down
18 changes: 9 additions & 9 deletions src/core/global_planner/evolutionary_planner/src/ga.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,8 @@ GA::~GA()
*/
bool GA::plan(const Node& start, const Node& goal, std::vector<Node>& path, std::vector<Node>& expand)
{
start_ = std::pair<double, double>(static_cast<double>(start.x_), static_cast<double>(start.y_));
goal_ = std::pair<double, double>(static_cast<double>(goal.x_), static_cast<double>(goal.y_));
start_ = std::pair<double, double>(static_cast<double>(start.x()), static_cast<double>(start.y()));
goal_ = std::pair<double, double>(static_cast<double>(goal.x()), static_cast<double>(goal.y()));
expand.clear();

if ((n_genets_ <= 0) || (n_genets_ % 2 != 0))
Expand Down Expand Up @@ -134,10 +134,10 @@ bool GA::plan(const Node& start, const Node& goal, std::vector<Node>& path, std:

// Generating Paths from Optimal Genets
std::vector<std::pair<double, double>> points, b_path;
points.emplace_back(static_cast<double>(start.x_), static_cast<double>(start.y_));
points.emplace_back(static_cast<double>(start.x()), static_cast<double>(start.y()));
for (const auto& pos : best_genet.position)
points.emplace_back(static_cast<double>(pos.first), static_cast<double>(pos.second));
points.emplace_back(static_cast<double>(goal.x_), static_cast<double>(goal.y_));
points.emplace_back(static_cast<double>(goal.x()), static_cast<double>(goal.y()));
points.erase(std::unique(std::begin(points), std::end(points)), std::end(points));

bspline_gen_.run(points, b_path);
Expand All @@ -157,7 +157,7 @@ bool GA::plan(const Node& start, const Node& goal, std::vector<Node>& path, std:
int x = static_cast<int>(b_path[p].first);
int y = static_cast<int>(b_path[p].second);
// Check if the current point is different from the last point
if (x != path.back().x_ || y != path.back().y_)
if (x != path.back().x() || y != path.back().y())
{
path.emplace_back(x, y, 0.0, 0.0, p, 0);
}
Expand Down Expand Up @@ -191,17 +191,17 @@ void GA::initializePositions(PositionSequence& initial_positions, const Node& st
int point_id, pos_id;

// Calculate sequence direction
bool x_order = (goal.x_ > start.x_);
bool y_order = (goal.y_ > start.y_);
bool x_order = (goal.x() > start.x());
bool y_order = (goal.y() > start.y());

// circle generation
int center_x, center_y;
double radius;
if (gen_mode == GEN_MODE_CIRCLE)
{
// Calculate the center and the radius of the circle (midpoint between start and goal)
center_x = (start.x_ + goal.x_) / 2;
center_y = (start.y_ + goal.y_) / 2;
center_x = (start.x() + goal.x()) / 2;
center_y = (start.y() + goal.y()) / 2;
radius = helper::dist(start, goal) / 2.0 < 5 ? 5 : helper::dist(start, goal) / 2.0;
}

Expand Down
18 changes: 9 additions & 9 deletions src/core/global_planner/evolutionary_planner/src/pso.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,8 @@ PSO::~PSO()
*/
bool PSO::plan(const Node& start, const Node& goal, std::vector<Node>& path, std::vector<Node>& expand)
{
start_ = std::pair<double, double>(static_cast<double>(start.x_), static_cast<double>(start.y_));
goal_ = std::pair<double, double>(static_cast<double>(goal.x_), static_cast<double>(goal.y_));
start_ = std::pair<double, double>(static_cast<double>(start.x()), static_cast<double>(start.y()));
goal_ = std::pair<double, double>(static_cast<double>(goal.x()), static_cast<double>(goal.y()));
expand.clear();

// variable initialization
Expand Down Expand Up @@ -117,10 +117,10 @@ bool PSO::plan(const Node& start, const Node& goal, std::vector<Node>& path, std

// Generating Paths from Optimal Particles
std::vector<std::pair<double, double>> points, b_path;
points.emplace_back(static_cast<double>(start.x_), static_cast<double>(start.y_));
points.emplace_back(static_cast<double>(start.x()), static_cast<double>(start.y()));
for (const auto& pos : best_particle.position)
points.emplace_back(static_cast<double>(pos.first), static_cast<double>(pos.second));
points.emplace_back(static_cast<double>(goal.x_), static_cast<double>(goal.y_));
points.emplace_back(static_cast<double>(goal.x()), static_cast<double>(goal.y()));
points.erase(std::unique(std::begin(points), std::end(points)), std::end(points));

bspline_gen_.run(points, b_path);
Expand All @@ -141,7 +141,7 @@ bool PSO::plan(const Node& start, const Node& goal, std::vector<Node>& path, std
int y = static_cast<int>(b_path[p].second);

// Check if the current point is different from the last point
if (x != path.back().x_ || y != path.back().y_)
if (x != path.back().x() || y != path.back().y())
path.emplace_back(x, y, 0.0, 0.0, p, 0);
}
}
Expand Down Expand Up @@ -173,17 +173,17 @@ void PSO::initializePositions(PositionSequence& initial_positions, const Node& s
int point_id, pos_id;

// Calculate sequence direction
bool x_order = (goal.x_ > start.x_);
bool y_order = (goal.y_ > start.y_);
bool x_order = (goal.x() > start.x());
bool y_order = (goal.y() > start.y());

// circle generation
int center_x, center_y;
double radius;
if (gen_mode == GEN_MODE_CIRCLE)
{
// Calculate the center and the radius of the circle (midpoint between start and goal)
center_x = (start.x_ + goal.x_) / 2;
center_y = (start.y_ + goal.y_) / 2;
center_x = (start.x() + goal.x()) / 2;
center_y = (start.y() + goal.y()) / 2;
radius = helper::dist(start, goal) / 2.0 < 5 ? 5 : helper::dist(start, goal) / 2.0;
}

Expand Down
6 changes: 3 additions & 3 deletions src/core/global_planner/global_planner/src/global_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -125,11 +125,11 @@ std::vector<Node> GlobalPlanner::_convertClosedListToPath(std::unordered_map<int
const Node& goal)
{
std::vector<Node> path;
auto current = closed_list.find(goal.id_);
auto current = closed_list.find(goal.id());
while (current->second != start)
{
path.emplace_back(current->second.x_, current->second.y_);
auto it = closed_list.find(current->second.pid_);
path.emplace_back(current->second.x(), current->second.y());
auto it = closed_list.find(current->second.pid());
if (it != closed_list.end())
current = it;
else
Expand Down
20 changes: 10 additions & 10 deletions src/core/global_planner/graph_planner/src/a_star.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -74,10 +74,10 @@ bool AStar::plan(const Node& start, const Node& goal, std::vector<Node>& path, s
open_list.pop();

// current node does not exist in closed list
if (closed_list.find(current.id_) != closed_list.end())
if (closed_list.find(current.id()) != closed_list.end())
continue;

closed_list.insert(std::make_pair(current.id_, current));
closed_list.insert(std::make_pair(current.id(), current));
expand.push_back(current);

// goal found
Expand All @@ -92,28 +92,28 @@ bool AStar::plan(const Node& start, const Node& goal, std::vector<Node>& path, s
{
// explore a new node
Node node_new = current + motion;
node_new.id_ = grid2Index(node_new.x_, node_new.y_);
node_new.set_id(grid2Index(node_new.x(), node_new.y()));

// node_new in closed list
if (closed_list.find(node_new.id_) != closed_list.end())
if (closed_list.find(node_new.id()) != closed_list.end())
continue;

node_new.pid_ = current.id_;
node_new.set_pid(current.id());

// next node hit the boundary or obstacle
// prevent planning failed when the current within inflation
if ((node_new.id_ < 0) || (node_new.id_ >= map_size_) ||
(costmap_->getCharMap()[node_new.id_] >= costmap_2d::LETHAL_OBSTACLE * factor_ &&
costmap_->getCharMap()[node_new.id_] >= costmap_->getCharMap()[current.id_]))
if ((node_new.id() < 0) || (node_new.id() >= map_size_) ||
(costmap_->getCharMap()[node_new.id()] >= costmap_2d::LETHAL_OBSTACLE * factor_ &&
costmap_->getCharMap()[node_new.id()] >= costmap_->getCharMap()[current.id()]))
continue;

// if using dijkstra implementation, do not consider heuristics cost
if (!is_dijkstra_)
node_new.h_ = helper::dist(node_new, goal);
node_new.set_h(helper::dist(node_new, goal));

// if using GBFS implementation, only consider heuristics cost
if (is_gbfs_)
node_new.g_ = 0.0;
node_new.set_g(0.0);
// else, g will be calculate through node_new = current + m

open_list.push(node_new);
Expand Down
Loading

0 comments on commit 033a80c

Please sign in to comment.