bool AstarSearch::setGoalNode()
{
// Get index of goal pose
int index_x;
int index_y;
int index_theta;
poseToIndex(goal_pose_local_.pose, &index_x, &index_y, &index_theta);
SimpleNode goal_sn(index_x, index_y, index_theta, 0, 0);
// Check if goal is valid
if (isOutOfRange(index_x, index_y) || detectCollision(goal_sn))
return false;
// Calculate wavefront heuristic cost
if (use_wavefront_heuristic_) {
bool wavefront_result = calcWaveFrontHeuristic(goal_sn);
if (!wavefront_result) {
ROS_WARN("Goal is not reachable...");
return false;
}
}
return true;
}
bool AstarSearch::setStartNode()
{
// Get index of start pose
int index_x;
int index_y;
int index_theta;
poseToIndex(start_pose_local_.pose, &index_x, &index_y, &index_theta);
SimpleNode start_sn(index_x, index_y, index_theta, 0, 0);
// Check if start is valid
if (isOutOfRange(index_x, index_y) || detectCollision(start_sn))
return false;
// Set start node
AstarNode &start_node = nodes_[index_y][index_x][index_theta];
start_node.x = start_pose_local_.pose.position.x;
start_node.y = start_pose_local_.pose.position.y;
start_node.theta = 2.0 * M_PI / angle_size_ * index_theta;
start_node.gc = 0;
start_node.back = false;
start_node.status = STATUS::OPEN;
if (!use_wavefront_heuristic_)
start_node.hc = astar::calcDistance(start_pose_local_.pose.position.x, start_pose_local_.pose.position.y, goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y);
// Push start node to openlist
start_sn.cost = start_node.gc + start_node.hc;
openlist_.push(start_sn);
return true;
}
bool AstarSearch::detectCollision(const SimpleNode &sn)
{
// Define the robot as rectangle
static double left = -1.0 * base2back_;
static double right = robot_length_ - base2back_;
static double top = robot_width_ / 2.0;
static double bottom = -1.0 * robot_width_ / 2.0;
static double resolution = map_info_.resolution;
// Coordinate of base_link in OccupancyGrid frame
static double one_angle_range = 2.0 * M_PI / angle_size_;
double base_x = sn.index_x * resolution;
double base_y = sn.index_y * resolution;
double base_theta = sn.index_theta * one_angle_range;
// Calculate cos and sin in advance
double cos_theta = std::cos(base_theta);
double sin_theta = std::sin(base_theta);
// Convert each point to index and check if the node is Obstacle
for (double x = left; x < right; x += resolution) {
for (double y = top; y > bottom; y -= resolution) {
// 2D point rotation
int index_x = (x * cos_theta - y * sin_theta + base_x) / resolution;
int index_y = (x * sin_theta + y * cos_theta + base_y) / resolution;
if (isOutOfRange(index_x, index_y))
return true;
if (nodes_[index_y][index_x][0].status == STATUS::OBS)
return true;
}
}
return false;
}
void BallDialog::paintEvent( QPaintEvent *) {
p->begin( this );
p->setBrush( Qt::SolidPattern );
p->setPen( Qt::green );
if(isOutOfRange())
{
x = x = width()/2;
y = height()/2;
}
p->drawEllipse( QPoint( x,y ), xradius, yradius);
p->end();
}
// Simple collidion detection for wavefront search
bool AstarSearch::detectCollisionWaveFront(const WaveFrontNode &ref)
{
// Define the robot as square
static double half = robot_width_ / 2;
double robot_x = ref.index_x * map_info_.resolution;
double robot_y = ref.index_y * map_info_.resolution;
for (double y = half; y > -1.0 * half; y -= map_info_.resolution) {
for (double x = -1.0 * half; x < half; x += map_info_.resolution) {
int index_x = (robot_x + x) / map_info_.resolution;
int index_y = (robot_y + y) / map_info_.resolution;
if (isOutOfRange(index_x, index_y))
return true;
if (nodes_[index_y][index_x][0].status == STATUS::OBS)
return true;
}
}
return false;
}
bool AstarSearch::search()
{
// -- Start Astar search ----------
// If the openlist is empty, search failed
while (!openlist_.empty()) {
// Terminate the search if the count reaches a certain value
static int search_count = 0;
search_count++;
if (search_count > 300000) {
ROS_WARN("Exceed time limit");
search_count = 0;
return false;
}
// Pop minimum cost node from openlist
SimpleNode sn;
sn = openlist_.top();
openlist_.pop();
nodes_[sn.index_y][sn.index_x][sn.index_theta].status = STATUS::CLOSED;
// Expand nodes from this node
AstarNode *current_node = &nodes_[sn.index_y][sn.index_x][sn.index_theta];
// for each update
for (const auto &state : state_update_table_[sn.index_theta]) {
// Next state
double next_x = current_node->x + state.shift_x;
double next_y = current_node->y + state.shift_y;
double next_theta = astar::modifyTheta(current_node->theta + state.rotation);
double move_cost = state.step;
#if DEBUG
// Display search process
geometry_msgs::Pose p;
p.position.x = next_x;
p.position.y = next_y;
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(next_theta), p.orientation);
p = astar::transformPose(p, map2ogm_);
debug_pose_array_.poses.push_back(p);
#endif
// Increase curve cost
if (state.curve)
move_cost *= curve_weight_;
// Increase reverse cost
if (current_node->back != state.back)
move_cost *= reverse_weight_;
// Calculate index of the next state
SimpleNode next;
next.index_x = next_x / map_info_.resolution;
next.index_y = next_y / map_info_.resolution;
next.index_theta = sn.index_theta + state.index_theta;
// Avoid invalid index
next.index_theta = (next.index_theta + angle_size_) % angle_size_;
// Check if the index is valid
if (isOutOfRange(next.index_x, next.index_y) || detectCollision(next))
continue;
AstarNode *next_node = &nodes_[next.index_y][next.index_x][next.index_theta];
double next_hc = nodes_[next.index_y][next.index_x][0].hc;
// Calculate euclid distance heuristic cost
if (!use_wavefront_heuristic_)
next_hc = astar::calcDistance(next_x, next_y, goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y);
// GOAL CHECK
if (isGoal(next_x, next_y, next_theta)) {
search_count = 0;
next_node->status = STATUS::OPEN;
next_node->x = next_x;
next_node->y = next_y;
next_node->theta = next_theta;
next_node->gc = current_node->gc + move_cost;
next_node->hc = next_hc;
next_node->back = state.back;
next_node->parent = current_node;
setPath(next);
return true;
}
// NONE
if (next_node->status == STATUS::NONE) {
next_node->status = STATUS::OPEN;
next_node->x = next_x;
next_node->y = next_y;
next_node->theta = next_theta;
next_node->gc = current_node->gc + move_cost;
next_node->hc = next_hc;
next_node->back = state.back;
next_node->parent = current_node;
next.cost = next_node->gc + next_node->hc;
openlist_.push(next);
continue;
}
// OPEN or CLOSED
if (next_node->status == STATUS::OPEN || next_node->status == STATUS::CLOSED) {
if (current_node->gc + move_cost + next_hc < next_node->gc + next_hc) {
next_node->status = STATUS::OPEN;
next_node->x = next_x;
next_node->y = next_y;
next_node->theta = next_theta;
next_node->gc = current_node->gc + move_cost;
next_node->hc = next_hc;
next_node->back = state.back;
next_node->parent = current_node;
next.cost = next_node->gc + next_node->hc;
openlist_.push(next);
continue;
}
}
if (search_count == 0)
break;
} // state update
}
// Failed to find path
ROS_WARN("Openlist is Empty!");
return false;
}
bool AstarSearch::calcWaveFrontHeuristic(const SimpleNode &sn)
{
// Set start point for wavefront search
// This is goal for Astar search
nodes_[sn.index_y][sn.index_x][0].hc = 0;
WaveFrontNode wf_node(sn.index_x, sn.index_y, 1e-10);
std::queue<WaveFrontNode> qu;
qu.push(wf_node);
// State update table for wavefront search
// Nodes are expanded for each neighborhood cells (moore neighborhood)
double resolution = map_info_.resolution;
static std::vector<WaveFrontNode> updates = {
astar::getWaveFrontNode( 0, 1, resolution),
astar::getWaveFrontNode(-1, 0, resolution),
astar::getWaveFrontNode( 1, 0, resolution),
astar::getWaveFrontNode( 0, -1, resolution),
astar::getWaveFrontNode(-1, 1, std::hypot(resolution, resolution)),
astar::getWaveFrontNode( 1, 1, std::hypot(resolution, resolution)),
astar::getWaveFrontNode(-1, -1, std::hypot(resolution, resolution)),
astar::getWaveFrontNode( 1, -1, std::hypot(resolution, resolution)),
};
// Get start index
int start_index_x;
int start_index_y;
int start_index_theta;
poseToIndex(start_pose_local_.pose, &start_index_x, &start_index_y, &start_index_theta);
// Whether the robot can reach goal
bool reachable = false;
// Start wavefront search
while (!qu.empty()) {
WaveFrontNode ref = qu.front();
qu.pop();
WaveFrontNode next;
for (const auto &u : updates) {
next.index_x = ref.index_x + u.index_x;
next.index_y = ref.index_y + u.index_y;
// out of range OR already visited OR obstacle node
if (isOutOfRange(next.index_x, next.index_y) ||
nodes_[next.index_y][next.index_x][0].hc > 0 ||
nodes_[next.index_y][next.index_x][0].status == STATUS::OBS)
continue;
// Take the size of robot into account
if (detectCollisionWaveFront(next))
continue;
// Check if we can reach from start to goal
if (next.index_x == start_index_x && next.index_y == start_index_y)
reachable = true;
// Set wavefront heuristic cost
next.hc = ref.hc + u.hc;
nodes_[next.index_y][next.index_x][0].hc = next.hc;
qu.push(next);
}
}
// End of search
return reachable;
}
