基于C++实现BFS的最短路径搜索时，可以使用STL中的优先队列priority_queue，优先队列按照小顶堆，即路径短的优先取出。其中节点可以用结构体定义，结构体中存储节点的位置、路径长度、最短路径的前序节点等信息，构建结构体的时候记得重载小于号，即可直接在优先队列中使用，具体代码如下：

其中，起点为左上角，终点为右下角，障碍物通过值设置为9999，可以根据题目的要求设置为一个非常大的值

#include<bits/stdc++.h>
using namespace std;
struct node{
  int x;
  int y;
  int prex;///前序节点x
  int prey;///前序节点y
  int val;///到该处路径的代价
  bool operator < (const node &x) const{
        return val > x.val;
  }
}S[16][16];
vector<vector<int> > array(16);
priority_queue<node> q;
int M,N;
void judge(int xadd, int yadd, int val, int px, int py){
    int x = px + xadd;
    int y = py + yadd;
    if(x<0||y<0||x>=M||y>=N){
      return;
    }
   // cout<<val<<" "<<array[x][y]<<" "<<S[x][y].val<<endl;
    if(val+array[x][y]<S[x][y].val){
       S[x][y].val = val+array[x][y];
       S[x][y].prex = px;
       S[x][y].prey = py;
       q.push(S[x][y]);
    }
    return;
}
void route(int x, int y){
    if(x<0||y<0){
        return;
    }
    route(S[x][y].prex, S[x][y].prey);
    if(!(x==0&&y==0)){
       cout<<" ";
    }
    cout<<"("<<x<<","<<y<<")";
}
int main(){
    while(cin>>M>>N){
        ///数组读取
        for(int i=0;i<M;i++){
            array[i].clear();
            array[i].resize(N);
            for(int j=0;j<N;j++){
               cin>>array[i][j];
               S[i][j].x = i;
               S[i][j].y = j;
               S[i][j].prex = -1;
               S[i][j].prey = -1;
               S[i][j].val = 9999;
            }
        }
        ///最短路
        while(!q.empty()) q.pop();
        S[0][0].val = 0;
        q.push(S[0][0]);
        while(!q.empty()){
          //  cout<<q.top().x<<" "<<q.top().y<<endl;
            node temp = q.top();
            q.pop();
            judge(-1, 0, temp.val, temp.x, temp.y);
            judge( 1, 0, temp.val, temp.x, temp.y);
            judge( 0, 1, temp.val, temp.x, temp.y);
            judge( 0,-1, temp.val, temp.x, temp.y);
        }
        if(S[M-1][N-1].val>5000){
          cout<<"-1"<<endl;
        }else{
          route(M-1,N-1);
          cout<<endl;
        }
    }
}
/*
7 5
0 1 1 1 1
8 1 4 9 7
4 9999 9999 5 1
1 1 6 9999 6
2 9999 3 9999 4
4 3 9999 5 3
4 2 3 2 0



*/