diff --git a/CMakeLists.txt b/CMakeLists.txt index dea0e84..d0be506 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.13) project(WarthogJPS - VERSION 0.5.0 + VERSION 0.5.1 LANGUAGES CXX C) set_property(GLOBAL PROPERTY WARTHOG_warthog-jps ON) diff --git a/apps/jps.cpp b/apps/jps.cpp index 2d8d29d..5448d47 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -15,6 +15,9 @@ #include #include #include +#ifdef WARTHOG_POSTHOC +#include +#endif #include #include @@ -23,12 +26,14 @@ #include "cfg.h" #include +#include #include #include #include #include #include +#include #include #include #include @@ -43,6 +48,16 @@ int checkopt = 0; int verbose = 0; // display program help on startup int print_help = 0; +// run only this query, or -1 for all +int filter_id = -1; +#ifdef WARTHOG_POSTHOC +// write trace to file, empty string to disable +std::string trace_file; +using listener_grid = ::warthog::io::octile_grid_trace; +using listener_type = std::tuple; +#else +using listener_type = std::tuple<>; +#endif void help(std::ostream& out) @@ -65,6 +80,11 @@ help(std::ostream& out) "values in the scen file)\n" << "\t--verbose (optional; prints debugging info when compiled " "with debug symbols)\n" + << "\t--filter [id] (optional; run only query [id])\n" +#ifdef WARTHOG_POSTHOC + << "\t--trace [.trace.yaml file] (optional; write posthoc trace for " + "first query to [file])\n" +#endif << "Invoking the program this way solves all instances in [scen " "file] with algorithm [alg]\n" << "Currently recognised values for [alg]:\n" @@ -87,7 +107,15 @@ check_optimality( if(fabs(delta - epsilon) > epsilon) { - std::cerr << std::setprecision(15); + std::stringstream strpathlen; + strpathlen << std::fixed << std::setprecision(exp->precision()); + strpathlen << sol.sum_of_edge_costs_; + + std::stringstream stroptlen; + stroptlen << std::fixed << std::setprecision(exp->precision()); + stroptlen << exp->distance(); + + std::cerr << std::setprecision(exp->precision()); std::cerr << "optimality check failed!" << std::endl; std::cerr << std::endl; std::cerr << "optimal path length: " << exp->distance() @@ -96,11 +124,17 @@ check_optimality( std::cerr << "precision: " << precision << " epsilon: " << epsilon << std::endl; std::cerr << "delta: " << delta << std::endl; - exit(1); + return false; } return true; } +#ifdef WARTHOG_POSTHOC +#define WARTHOG_POSTHOC_DO(f) f +#else +#define WARTHOG_POSTHOC_DO(f) +#endif + template int run_experiments( @@ -108,12 +142,36 @@ run_experiments( warthog::util::scenario_manager& scenmgr, bool verbose, bool checkopt, std::ostream& out) { + WARTHOG_GINFO_FMT("start search with algorithm {}", alg_name); + warthog::search::search_parameters par; + warthog::search::solution sol; auto* expander = algo.get_expander(); if(expander == nullptr) return 1; + out << "id\talg\texpanded\tgenerated\treopen\tsurplus\theapops" << "\tnanos\tplen\tpcost\tscost\tmap\n"; - for(unsigned int i = 0; i < scenmgr.num_experiments(); i++) + for(uint32_t i = filter_id >= 0 ? static_cast(filter_id) : 0, + ie = filter_id >= 0 + ? i + 1 + : static_cast(scenmgr.num_experiments()); + i < ie; i++) { +#ifdef WARTHOG_POSTHOC + std::optional + trace_stream; // open and pass to trace if used + if constexpr(std::same_as< + listener_type, + std::remove_cvref_t>) + { + if(i == filter_id && !trace_file.empty()) + { + listener_grid& l + = std::get(algo.get_listeners()); + trace_stream.emplace(trace_file); + l.open(*trace_stream); + } + } +#endif warthog::util::experiment* exp = scenmgr.get_experiment(i); warthog::pack_id startid @@ -121,11 +179,23 @@ run_experiments( warthog::pack_id goalid = expander->get_pack(exp->goalx(), exp->goaly()); warthog::search::problem_instance pi(startid, goalid, verbose); - warthog::search::search_parameters par; - warthog::search::solution sol; + sol.reset(); algo.get_path(&pi, &par, &sol); +#ifdef WARTHOG_POSTHOC + if constexpr(std::same_as< + listener_type, + std::remove_cvref_t>) + { + if(trace_stream.has_value()) + { + // close + std::get(algo.get_listeners()).close(); + } + } +#endif + out << i << "\t" << alg_name << "\t" << sol.met_.nodes_expanded_ << "\t" << sol.met_.nodes_generated_ << "\t" << sol.met_.nodes_reopen_ << "\t" << sol.met_.nodes_surplus_ @@ -137,10 +207,16 @@ run_experiments( if(checkopt) { - if(!check_optimality(sol, exp)) return 4; + if(!check_optimality(sol, exp)) + { + WARTHOG_GCRIT("search error: failed suboptimal 4"); + return 4; + } } } + WARTHOG_GINFO_FMT( + "search complete; total memory: {}", algo.mem() + scenmgr.mem()); return 0; } @@ -155,17 +231,12 @@ run_jps( warthog::heuristic::octile_heuristic heuristic(map.width(), map.height()); warthog::util::pqueue_min open; - warthog::search::unidirectional_search jps(&heuristic, &expander, &open); + warthog::search::unidirectional_search jps( + &heuristic, &expander, &open, listener_type(WARTHOG_POSTHOC_DO(&map))); int ret = run_experiments( jps, alg_name, scenmgr, verbose, checkopt, std::cout); - if(ret != 0) - { - std::cerr << "run_experiments error code " << ret << std::endl; - return ret; - } - std::cerr << "done. total memory: " << jps.mem() + scenmgr.mem() << "\n"; - return 0; + return ret; } } // namespace @@ -175,13 +246,17 @@ main(int argc, char** argv) { // parse arguments warthog::util::param valid_args[] - = {{"alg", required_argument, 0, 1}, + = {{"alg", required_argument, 0, 0}, {"scen", required_argument, 0, 0}, - {"map", required_argument, 0, 1}, + {"map", required_argument, 0, 0}, // {"gen", required_argument, 0, 3}, {"help", no_argument, &print_help, 1}, {"checkopt", no_argument, &checkopt, 1}, {"verbose", no_argument, &verbose, 1}, + {"filter", required_argument, &filter_id, 1}, +#ifdef WARTHOG_POSTHOC + {"trace", required_argument, 0, 0}, +#endif {"costs", required_argument, 0, 1}, {0, 0, 0, 0}}; @@ -191,7 +266,7 @@ main(int argc, char** argv) if(argc == 1 || print_help) { help(std::cout); - exit(0); + return 0; } std::string sfile = cfg.get_param_value("scen"); @@ -200,6 +275,14 @@ main(int argc, char** argv) std::string mapfile = cfg.get_param_value("map"); std::string costfile = cfg.get_param_value("costs"); + if(filter_id == 1) + { + filter_id = std::stoi(cfg.get_param_value("filter")); + } +#ifdef WARTHOG_POSTHOC + trace_file = cfg.get_param_value("trace"); +#endif + // if(gen != "") // { // warthog::util::scenario_manager sm; @@ -213,7 +296,7 @@ main(int argc, char** argv) if(alg == "" || sfile == "") { help(std::cout); - exit(0); + return 0; } // load up the instances @@ -223,12 +306,13 @@ main(int argc, char** argv) if(scenmgr.num_experiments() == 0) { std::cerr << "err; scenario file does not contain any instances\n"; - exit(0); + return 1; } // the map filename can be given or (default) taken from the scenario file if(mapfile == "") { + // first, try to load the map from the scenario file mapfile = warthog::util::find_map_filename(scenmgr, sfile); if(mapfile.empty()) { diff --git a/cmake/headers.cmake b/cmake/headers.cmake index 50b5884..203e310 100644 --- a/cmake/headers.cmake +++ b/cmake/headers.cmake @@ -6,6 +6,8 @@ include/jps/forward.h include/jps/domain/rotate_gridmap.h +include/jps/io/octile_grid_trace.h + include/jps/jump/block_online.h include/jps/jump/jump.h include/jps/jump/jump_point_offline.h diff --git a/extern/warthog-core b/extern/warthog-core index f379ef2..ba9b5f6 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit f379ef257335bb8bb6645c6efaeef031763273ea +Subproject commit ba9b5f66281ca6a767360787037ae80a7226e38e diff --git a/include/jps/io/octile_grid_trace.h b/include/jps/io/octile_grid_trace.h new file mode 100644 index 0000000..b7d638c --- /dev/null +++ b/include/jps/io/octile_grid_trace.h @@ -0,0 +1,108 @@ +#ifndef JPS_IO_OCTILE_GRID_TRACE_H +#define JPS_IO_OCTILE_GRID_TRACE_H + +// io/octile_grid_trace.h +// +// Adds support for Octile grid trace, draws successor lines intercardinal then +// cardinal. +// +// @author: Ryan Hechenberger +// @created: 2025-08-07 +// + +#include + +namespace warthog::io +{ + +/// @brief class that produces a posthoc trace for the gridmap domain, grid +/// must be set. +class octile_grid_trace : public grid_trace +{ +public: + using node = search::search_node; + + using grid_trace::grid_trace; + + void + print_posthoc_header() override; + +protected: + domain::gridmap* grid_; +}; + +inline void +octile_grid_trace::print_posthoc_header() +{ + if(*this) + { + stream() << R"posthoc(version: 1.4.0 +views: + cell: + - $: rect + width: 1 + height: 1 + x: ${{$.x}} + y: ${{$.y}} + fill: ${{$.fill}} + clear: ${{$.clear}} + succesor: + - $: cell + x: ${{$.x}} + y: ${{$.y}} + fill: ${{$.fill}} + clear: ${{$.clear}} + - $: drawindirect + $if: ${{ !!parent }} + x: ${{$.x}} + y: ${{$.y}} + dx: ${{$.x-parent.x}} + dy: ${{$.y-parent.y}} + fill: ${{$.fill}} + drawindirect: + - $: path + points: [ { x: "${{$.x + 0.5}}", y: "${{$.y + 0.5}}" }, + { x: "${{ parent.x + 0.5 + ( Math.abs($.dx) < Math.abs($.dy) ? $.dx : Math.sign($.dx) * Math.abs($.dy) ) }}", + y: "${{ parent.y + 0.5 + ( Math.abs($.dy) < Math.abs($.dx) ? $.dy : Math.sign($.dy) * Math.abs($.dx) ) }}" }, + { x: "${{parent.x + 0.5}}", y: "${{parent.y + 0.5}}" } + ] + fill: ${{$.fill}} + line-width: 0.25 + clear: ${{$.clear}} + main: + - $: cell + $if: ${{ $.type == 'source' }} + fill: green + clear: false + - $: cell + $if: ${{ $.type == 'destination' }} + fill: red + clear: false + - $: cell + $if: ${{ $.type == 'expand' }} + fill: cyan + clear: false + - $: cell + $if: ${{ $.type == 'expand' }} + fill: blue + clear: close + - $: cell + $if: ${{ $.type == 'generate' }} + fill: purple + clear: false + - $: succesor + $if: ${{ $.type == 'generate' }} + fill: orange + clear: close +pivot: + x: ${{ $.x + 0.5 }} + y: ${{ $.y + 0.5 }} + scale: 1 +events: +)posthoc"; + } +} + +} // namespace warthog::io + +#endif // WARTHOG_IO_GRID_TRACE_H