WIP: new visitors for cost & time

descartes_light/bgl/include/descartes_light/bgl/event_visitors.h

@@ -1,8 +1,8 @@
 #ifndef DESCARTES_LIGHT_SOLVERS_BGL_EVENT_VISITORS
 #define DESCARTES_LIGHT_SOLVERS_BGL_EVENT_VISITORS
 
+#include <chrono>
 #include <descartes_light/bgl/boost_graph_types.h>
-
 #include <descartes_light/descartes_macros.h>
 DESCARTES_IGNORE_WARNINGS_PUSH
 #include <boost/graph/visitors.hpp>
@@ -29,6 +29,54 @@ struct early_terminator : public boost::base_visitor<early_terminator<EventType>
   const long last_rung_idx_;
 };
 
+/**
+ * @brief Event visitor that terminates the search when a vertex in the last rung of the graph is found
+ * to be below the provided cost threshold
+ * @details Throws the vertex descriptor that is the termination of the path a valid vertex in the last rung
+ * is found
+ */
+template <typename FloatType, typename EventType>
+struct distance_terminator : public boost::base_visitor<distance_terminator<FloatType, EventType>>
+{
+  /** @brief Event filter typedef defining the events for which this visitor can be used */
+  typedef EventType event_filter;
+
+  distance_terminator(long last_rung_idx, FloatType distance_threshold);
+
+  void operator()(VertexDesc<FloatType> u, const BGLGraph<FloatType>& g);
+
+  const FloatType distance_threshold_;
+  const long last_rung_idx_;
+};
+
+
+/**
+ * @brief The timeout_exception class
+ */
+class timeout_exception : public std::runtime_error {
+public:
+  using std::runtime_error::runtime_error;
+};
+
+
+/**
+ * @brief Event visitor that terminates the once a time threshold is met
+ * @details How will this throw an end descriptor
+ */
+template <typename FloatType, typename EventType>
+struct time_terminator : public boost::base_visitor<time_terminator<FloatType, EventType>>
+{
+  /** @brief Event filter typedef defining the events for which this visitor can be used */
+  typedef EventType event_filter;
+
+  time_terminator(double time_threshold);
+
+  void operator()(VertexDesc<FloatType> u, const BGLGraph<FloatType>& g);
+
+  const std::chrono::time_point<std::chrono::steady_clock> start_time_;
+  const double time_threshold_;
+};
+
 /**
  * @brief Event visitor that adds all edges to each vertex dynamically as the vertex is discovered during the graph
  * search

descartes_light/bgl/include/descartes_light/bgl/impl/bgl_dfs_solver.hpp

@@ -54,6 +54,21 @@ static VertexDesc<FloatType> solveDFS(BGLGraph<FloatType>& graph,
     if (target != ladder_rungs.back().end() && graph[*target].distance < std::numeric_limits<FloatType>::max())
       throw *target;
   }
+  catch(const descartes_light::timeout_exception& te)
+  {
+    // If this has to be called in two places, make it a function.
+    auto target = std::min_element(ladder_rungs.back().begin(),
+                                   ladder_rungs.back().end(),
+                                   [&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
+                                     return graph[a].distance < graph[b].distance;
+                                   });
+
+    // Check that the identified lowest cost vertex is valid and has a cost less than inf
+    if (target != ladder_rungs.back().end() && graph[*target].distance < std::numeric_limits<FloatType>::max())
+      throw *target;
+
+  }
+
   catch (const VertexDesc<FloatType>& target)
   {
     return target;

descartes_light/bgl/include/descartes_light/bgl/impl/event_visitors.hpp

@@ -18,6 +18,33 @@ void early_terminator<EventType>::operator()(VertexDesc<FloatType> u, const BGLG
     throw u;
 }
 
+template <typename FloatType, typename EventType>
+distance_terminator<FloatType, EventType>::distance_terminator(long last_rung_idx, FloatType distance_threshold) : distance_threshold_(distance_threshold),
+                                                                                                                        last_rung_idx_(last_rung_idx)
+{
+}
+
+template <typename FloatType,typename EventType>
+void distance_terminator<FloatType, EventType>::operator()(VertexDesc<FloatType> u, const BGLGraph<FloatType>& g)
+{
+  if (g[u].rung_idx == last_rung_idx_ && g[u].distance < distance_threshold_)
+    throw u;
+}
+
+template <typename FloatType, typename EventType>
+time_terminator<FloatType, EventType>::time_terminator(double time_threshold) : time_threshold_(time_threshold)
+{
+  start_time_ = std::chrono::steady_clock::now();
+}
+
+template <typename FloatType, typename EventType>
+void time_terminator<FloatType, EventType>::operator()(VertexDesc<FloatType> u, const BGLGraph<FloatType>& g)
+{
+  //if (time_threshold_ < std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time_).count())
+    throw false;
+}
+
+
 template <typename FloatType, typename EventType>
 add_all_edges_dynamically<FloatType, EventType>::add_all_edges_dynamically(
     std::vector<typename EdgeEvaluator<FloatType>::ConstPtr> edge_eval,