diff --git a/descartes_light/bgl/include/descartes_light/bgl/event_visitors.h b/descartes_light/bgl/include/descartes_light/bgl/event_visitors.h index 694de41e..00b05528 100644 --- a/descartes_light/bgl/include/descartes_light/bgl/event_visitors.h +++ b/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 #include - #include DESCARTES_IGNORE_WARNINGS_PUSH #include @@ -29,6 +29,54 @@ struct early_terminator : public boost::base_visitor 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 +struct distance_terminator : public boost::base_visitor> +{ + /** @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 u, const BGLGraph& 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 +struct time_terminator : public boost::base_visitor> +{ + /** @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 u, const BGLGraph& g); + + const std::chrono::time_point 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 diff --git a/descartes_light/bgl/include/descartes_light/bgl/impl/bgl_dfs_solver.hpp b/descartes_light/bgl/include/descartes_light/bgl/impl/bgl_dfs_solver.hpp index f3ddbf40..b8e29d0c 100644 --- a/descartes_light/bgl/include/descartes_light/bgl/impl/bgl_dfs_solver.hpp +++ b/descartes_light/bgl/include/descartes_light/bgl/impl/bgl_dfs_solver.hpp @@ -54,6 +54,21 @@ static VertexDesc solveDFS(BGLGraph& graph, if (target != ladder_rungs.back().end() && graph[*target].distance < std::numeric_limits::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& a, const VertexDesc& 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::max()) + throw *target; + + } + catch (const VertexDesc& target) { return target; diff --git a/descartes_light/bgl/include/descartes_light/bgl/impl/event_visitors.hpp b/descartes_light/bgl/include/descartes_light/bgl/impl/event_visitors.hpp index 81a8ab35..b7e78109 100644 --- a/descartes_light/bgl/include/descartes_light/bgl/impl/event_visitors.hpp +++ b/descartes_light/bgl/include/descartes_light/bgl/impl/event_visitors.hpp @@ -18,6 +18,33 @@ void early_terminator::operator()(VertexDesc u, const BGLG throw u; } +template +distance_terminator::distance_terminator(long last_rung_idx, FloatType distance_threshold) : distance_threshold_(distance_threshold), + last_rung_idx_(last_rung_idx) +{ +} + +template +void distance_terminator::operator()(VertexDesc u, const BGLGraph& g) +{ + if (g[u].rung_idx == last_rung_idx_ && g[u].distance < distance_threshold_) + throw u; +} + +template +time_terminator::time_terminator(double time_threshold) : time_threshold_(time_threshold) +{ + start_time_ = std::chrono::steady_clock::now(); +} + +template +void time_terminator::operator()(VertexDesc u, const BGLGraph& g) +{ + //if (time_threshold_ < std::chrono::duration(std::chrono::steady_clock::now() - start_time_).count()) + throw false; +} + + template add_all_edges_dynamically::add_all_edges_dynamically( std::vector::ConstPtr> edge_eval,