If the end node is not reachable, it return the maximum cost.

src/dijkstra.cpp

@@ -106,6 +106,8 @@ double CDijkstra::find_shortest_path(Eigen::MatrixXd &graph,unsigned int start_n
     d2=new_d2;
   }
   path.clear();
+  if(w(1,end_node)==w(w.rows()-1,end_node) && w(1,end_node)==std::numeric_limits<double>::max()/2.0)
+    return std::numeric_limits<double>::max()/2.0;
   if(end_node==start_node)
     path.push_back(0); 
   else