1     start_ = start_in;
 2     goal_ = goal_in;
 3     n = grid.size();
 4 
 5     // Get possible motions--> right, down and left, up
 6 //    std::vector<Node> motion = GetMotion();
 7     // set the start_ node as the current vertex
 8     open_list_.push(start_);
 9     //qDebug()<<open_list_.top().cost_<<" "<<open_list_.top().h_cost_;
10 
11     // Main loop
12 
13     SetTimer(NULL, 0, 50, (TIMERPROC)TimerProc);
14 
15     while (!open_list_.empty())
16     {
17         // get the minist-cost node as a new one in each iteration
18         Node current = open_list_.top();
19         open_list_.pop();
20         // assign the order as the id
21         current.id_ = current.x_ * n + current.y_;
22 
23         // find if current is the goal node
24         if (CompareCoordinates(current, goal_))
25         {
26             // if yes, than the work is done and exit
27             closed_list_.push_back(current);
28             grid[current.x_][current.y_] = 2;
29             KillTimer(NULL, 0);
30             return closed_list_;
31         }
32         grid[current.x_][current.y_] = 2;  // Point opened
33 
34         // find all the neighbours and update their distance to the original node
35         for (const auto& m : motion)
36         {
37             Node new_point;
38             //qDebug()<<new_point.cost_<<" "<<new_point.h_cost_;
39             //qDebug()<<m.cost_<<" "<<m.h_cost_;
40             new_point = current + m;
41             //qDebug()<<new_point.cost_<<" "<<new_point.h_cost_;
42 
43             new_point.id_ = n * new_point.x_ + new_point.y_;
44             new_point.pid_ = current.id_;
45 
46             if (CompareCoordinates(new_point, goal_))
47             {
48                 open_list_.push(new_point);
49                 break;
50             }
51             // Check boundaries
52             if (new_point.x_ < 0 || new_point.y_ < 0 || new_point.x_ >= n || new_point.y_ >= n)
53             {
54                 continue;
55             }
56             // obstacle or visited
57             if (grid[new_point.x_][new_point.y_] != 0)
58             {
59                 continue;
60             }
61             // push all the mearsured node into a priority queue which the minist-cost one will be in the top
62             open_list_.push(new_point);
63         }
64         closed_list_.push_back(current);
65         // update the grid to get a dynamic-view
66 
67     }
68 
69     KillTimer(NULL, 0);
70 
71     // no solution founded
72     closed_list_.clear();
73     Node no_path_node(-1, -1, -1, -1, -1, -1);
74     closed_list_.push_back(no_path_node);