In this post, I want to make a little code review for a global A* planner I’ve written for a university project. I want to go through all the relevant parts of the implementation so that you can leave your suggestions for improvements in the commands. Describing the code here will also help me understand it in the future when I’ll revisit the implementation to integrate it into the Roboost project.
So, What is a global planner?
In mobile robotics, there are typically two different types of planners for pathfinding. One is considered the global planner, the other one the local planner. The global planner’s task is to find a suitable path from the robots location to a goal pose, usually in the global map of the robotic system (this might be the map of the insides of a building for example). The local planner on the other hand is responsible to follow the global path on the one hand, and to avoid dynamic obstacles on the other hand. As the local planner needs to have a faster reaction time, usually only the local sensor data is used to do the obstacle avoidance etc.
In the code below, you can see a implementation of such a global planner. In this case, it is located in a ROS2 node and uses the A* algorithm to find the shortest path from the current robot position to the goal position. The map is an OpenCV matrix, in which the occupied space is denoted as 1, the unoccupied space as 0 (these maps are usually called occupancy grid).
After the code block, the A* algorithm is explained and we will also take a look at interesting parts of this implementation.
bool GOTONode::update_global_planner()
{
// Set the robot's safety radius, maximum A* iterations, and global path granularity
robot_safety_radius_ = param_->robot_safety_radius;
max_astar_iterations_ = param_->max_astar_iterations;
global_path_granularity_ = param_->global_path_granularity;
// Define the Node format for A* algorithm: <pose, cost>
using Node = std::tuple<tuw::Pose2D, float>;
tuw::Pose2D current_pose;
// Use pose corresponding to the set parameter
if (param_->use_ground_truth) {
current_pose = ground_truth_;
} else {
current_pose = estimated_pose_;
}
// Convert the goal and start poses from world coordinates to map coordinates
tuw::Pose2D goal = figure_map_->w2m(goal_pose_.position());
tuw::Pose2D start = figure_map_->w2m(current_pose.position());
int startX = start.get_x();
int startY = start.get_y();
int goalX = goal.get_x();
int goalY = goal.get_y();
// Data structures for A* algorithm
struct CompareCost {
// Comparator for sorting nodes in the priority queue based on their cost
bool operator()(const Node& lhs, const Node& rhs) const {
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
std::priority_queue<Node, std::vector<Node>, CompareCost> openSet; // Priority queue for open set
std::unordered_map<int, std::unordered_map<int, float>> costSoFar; // Cost of reaching each node
std::unordered_map<int, std::unordered_map<int, Node>> cameFrom; // Parent node of each explored node
// Lambda function to generate successors for a given node
auto getSuccessors = [](int x, int y) {
// Define the eight possible successors of a node (top-left to bottom-right)
std::vector<std::pair<int, int>> successors {
{x-1, y-1}, {x, y-1}, {x+1, y-1},
{x-1, y}, {x+1, y},
{x-1, y+1}, {x, y+1}, {x+1, y+1}
};
return successors;
};
// Lambda function for heuristic (estimated distance from a node to the goal)
auto heuristic = [goalX, goalY](int x, int y) {
return std::hypot(goalX - x, goalY - y);
};
// Start node
openSet.push(std::make_tuple(start, 0));
costSoFar[startX][startY] = 0;
int iteration = 0;
int x = -1;
int y = -1;
// A* algorithm main loop
while (!openSet.empty()) {
if (iteration > max_astar_iterations_) {
// Maximum iterations reached without finding a path
global_path_.clear();
std::cout << "A* algorithm could not find a path within " << max_astar_iterations_ << " iterations." << std::endl;
return false;
}
// Get the node with the least cost from the open set
Node current = openSet.top();
openSet.pop();
x = std::get<0>(current).get_x();
y = std::get<0>(current).get_y();
// Check if we reached the goal
if (x == goalX && y == goalY)
break;
// Iterate over the neighbors of the current node
for (auto [nx, ny] : getSuccessors(x, y)) {
// Skip neighbors outside the map boundaries
if (nx < 0 || ny < 0 || nx >= distance_field_.cols || ny >= distance_field_.rows)
continue;
// Skip neighbors within the robot's safety radius
if (distance_field_.at<float>(ny, nx) <= robot_safety_radius_)
continue;
// Calculate the cost of the path to the neighbor
float newCost = costSoFar[x][y] + std::hypot(nx - x, ny - y);
if (!costSoFar.count(nx) || !costSoFar[nx].count(ny) || newCost < costSoFar[nx][ny]) {
// Update the cost and priority of the neighbor node
costSoFar[nx][ny] = newCost;
float priority = newCost + heuristic(nx, ny);
openSet.push(std::make_tuple(tuw::Pose2D(tuw::Point2D(nx, ny), 0), priority));
cameFrom[nx][ny] = current;
}
}
iteration++;
}
// Clear the global path before reconstructing it
global_path_.clear();
// Check if the goal is reached after the while loop
if (openSet.empty() && !(x == goalX && y == goalY)) {
std::cout << "A* algorithm could not find a path to the goal." << std::endl;
return false;
}
// Reconstruct the path by following the parent nodes from the goal to the start
x = goalX;
y = goalY;
// Create a Path message
nav_msgs::msg::Path path_msg;
path_msg.header.stamp = this->now();
path_msg.header.frame_id = "world";
tuw::Point2D last_position_added = tuw::Point2D(x, y);
while (!(x == startX && y == startY)) {
Node previous_node = cameFrom[x][y];
tuw::Point2D current_position = figure_map_->m2w(tuw::Point2D(x, y));
// Only add poses that are at least global_path_granularity_ meters apart
if (current_position.distanceTo(last_position_added) >= global_path_granularity_ || (x == goalX && y == goalY)) {
if (x == goalX && y == goalY)
global_path_.push_back(goal_pose_);
else
global_path_.push_back(figure_map_->m2w(std::get<0>(previous_node).position()));
last_position_added = current_position;
geometry_msgs::msg::PoseStamped pose_stamped;
pose_stamped.header.stamp = this->now();
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.set__x(global_path_.back().get_x());
pose_stamped.pose.position.set__y(global_path_.back().get_y());
path_msg.poses.push_back(pose_stamped);
}
x = std::get<0>(previous_node).get_x();
y = std::get<0>(previous_node).get_y();
}
// Add the start position and reverse the global path
global_path_.push_back(figure_map_->m2w(start.position()));
std::reverse(global_path_.begin(), global_path_.end());
std::reverse(path_msg.poses.begin(), path_msg.poses.end());
// Set the global path for visualization
planner_viz_->set_global_path(global_path_);
// Publish the global path as a Path message
global_path_pub_->publish(path_msg);
return true;
}
Basic A* Principle
The A* algorithm is based on a heuristic (in this case the distance to the goal) and a set of opened nodes. First, the start node is opened. The surrounding nodes are then put into an ordered list. Here, based on the heuristic, the node nearest to the goal is chosen to open on the next turn. This then goes on till the opened node is equal to the goal. When this is the case, all the chosen nodes are backtracked and put into the resulting path.
Lambda Functions in C++
Some nice aspects of this implementation are that for the successor generation and the heuristic evaluation, lambda functions are used. A lambda function is basically a normal function as you know it but packed inside a variable. The following syntax can be used to do so:
[capture](parameters) -> return_type { function_body }
The square brackets [] contain the capture list, which is a list of variables from the surrounding scope that the lambda function will have access to. The rest should be clear.
This was a little excuse on the basics of global planners in robotics. If you have any questions, feel free to leave a comment below, I’m more than happy to help.
And last but not least, here is a little video of our finished planner implementation:
If you’re still here, I’d have a few questions:
- How would you improve the code for readability?
- What would be the best way to speed up the A* implementation? (Do you maybe have suggestions on how to parallelize it?)
- Have you implemented a global planner yet? If so, what technique did you use?