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; }