fix astar and add dijkstra

Author: onlytailei

CMakeLists.txt

@@ -35,6 +35,9 @@ target_link_libraries(pf ${OpenCV_LIBS} )
 #PathPlanning
 ##############################
 
+add_executable(dijkstra src/dijkstra.cpp)
+target_link_libraries(dijkstra ${OpenCV_LIBS} )
+
 add_executable(astar src/a_star.cpp)
 target_link_libraries(astar ${OpenCV_LIBS} )
 

gif/astar.gif renamed to gif/dijkstra.gif

@@ -66,19 +66,23 @@ Find all the executable files in ***build/bin***.
 [Probabilistic Robotics](http://www.probabilistic-robotics.org/)
 
 # Path Planning
+
+## Dijkstra
+* blue point: the start point
+* red point: the goal point
+<img src="https://ram-lab.com/file/tailei/gif/dijkstra.gif" alt="dijkstra" width="400"/>
+
 ## A star
 * blue point: the start point
 * red point: the goal point
 <img src="https://ram-lab.com/file/tailei/gif/astar.gif" alt="astar" width="400"/>
 
-
 ## RRT
 * red circle: the start point
 * blue circle: the goal point
 * black circle: obstacles
 <img src="https://ram-lab.com/file/tailei/gif/rrt.gif" alt="rrt" width="400"/>
 
-
 ## Dynamic Window Approach
 * blue circle: the target point
 * red circle: the robot

readme.md

@@ -20,10 +20,10 @@ class Node{
 public:
   int x;
   int y;
-  float cost;
+  float sum_cost;
   Node* p_node;
 
-  Node(int x_, int y_, float cost_, Node* p_node_=NULL):x(x_), y(y_),cost(cost_), p_node(p_node_){};
+  Node(int x_, int y_, float sum_cost_=0, Node* p_node_=NULL):x(x_), y(y_), sum_cost(sum_cost_), p_node(p_node_){};
 };
 
 
@@ -91,8 +91,8 @@ bool verify_node(Node* node,
 }
 
 
-float calc_heristic(Node n1, Node n2, float w=1.0){
-  return w * std::sqrt(std::pow(n1.x-n2.x, 2)+std::pow(n1.y-n2.y, 2));
+float calc_heristic(Node* n1, Node* n2, float w=1.0){
+  return w * std::sqrt(std::pow(n1->x-n2->x, 2)+std::pow(n1->y-n2->y, 2));
 }
 
 std::vector<Node> get_motion_model(){
@@ -160,7 +160,10 @@ void a_star_planning(float sx, float sy,
                   cv::Scalar(0, 0, 255), -1);
 
   std::vector<std::vector<int> > visit_map(xwidth, vector<int>(ywidth, 0));
+  
+  std::vector<std::vector<float> > path_cost(xwidth, vector<float>(ywidth, std::numeric_limits<float>::max()));
 
+  path_cost[nstart->x][nstart->y] = 0;
 
   std::vector<std::vector<int> > obmap = calc_obstacle_map(
                                                   ox, oy,
@@ -170,7 +173,7 @@ void a_star_planning(float sx, float sy,
                                                   bg, img_reso);
 
   // NOTE: d_ary_heap should be a better choice here
-  auto cmp = [](const Node* left, const Node* right){return left->cost > right->cost;};
+  auto cmp = [](const Node* left, const Node* right){return left->sum_cost > right->sum_cost;};
   std::priority_queue<Node*, std::vector<Node*>, decltype(cmp)> pq(cmp);
 
   pq.push(nstart);
@@ -189,7 +192,7 @@ void a_star_planning(float sx, float sy,
     }
 
     if (node->x == ngoal->x && node->y==ngoal->y){
-      ngoal->cost = node->cost;
+      ngoal->sum_cost = node->sum_cost;
       ngoal->p_node = node;
       break;
     }
@@ -198,7 +201,7 @@ void a_star_planning(float sx, float sy,
       Node * new_node = new Node(
         node->x + motion[i].x,
         node->y + motion[i].y,
-        node->cost + motion[i].cost,
+        path_cost[node->x][node->y] + motion[i].sum_cost + calc_heristic(ngoal, node),
         node);
 
       if (!verify_node(new_node, obmap, min_ox, max_ox, min_oy, max_oy)){
@@ -216,22 +219,25 @@ void a_star_planning(float sx, float sy,
                     cv::Point((new_node->x+1)*img_reso, (new_node->y+1)*img_reso),
                     cv::Scalar(0, 255, 0));
 
-      // std::string int_count = std::to_string(count);
-      // cv::imwrite("./pngs/"+std::string(5-int_count.length(), '0').append(int_count)+".png", bg);
+      std::string int_count = std::to_string(count);
+      cv::imwrite("./pngs/"+std::string(5-int_count.length(), '0').append(int_count)+".png", bg);
       count++;
       cv::imshow("astar", bg);
       cv::waitKey(5);
 
-      pq.push(new_node);
+      if (path_cost[node->x][node->y]+motion[i].sum_cost < path_cost[new_node->x][new_node->y]){
+        path_cost[new_node->x][new_node->y]=path_cost[node->x][node->y]+motion[i].sum_cost; 
+        pq.push(new_node);
+      }
     }
   }
 
   calc_final_path(ngoal, reso, bg, img_reso);
   delete ngoal;
   delete nstart;
 
-  // std::string int_count = std::to_string(count);
-  // cv::imwrite("./pngs/"+std::string(5-int_count.length(), '0').append(int_count)+".png", bg);
+  std::string int_count = std::to_string(count);
+  cv::imwrite("./pngs/"+std::string(5-int_count.length(), '0').append(int_count)+".png", bg);
   cv::imshow("astar", bg);
   cv::waitKey(5);
 };