From 86a81d092b6702b7641cd82c9d8b4c67088029d3 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Wed, 25 Mar 2026 12:58:16 +1100 Subject: [PATCH 1/4] sync github actions with warthog-core (#26) --- .github/CODEOWNERS | 1 + .github/workflows/clang-format-apply.yml | 2 +- .github/workflows/clang-format-test.yml | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) create mode 100644 .github/CODEOWNERS diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 0000000..0c2ccb0 --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1 @@ +/.github/ @heavenfall diff --git a/.github/workflows/clang-format-apply.yml b/.github/workflows/clang-format-apply.yml index 194fff0..4c9a158 100644 --- a/.github/workflows/clang-format-apply.yml +++ b/.github/workflows/clang-format-apply.yml @@ -9,7 +9,7 @@ jobs: contents: write steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@v6 - uses: DoozyX/clang-format-lint-action@v0.18.1 with: source: '.' diff --git a/.github/workflows/clang-format-test.yml b/.github/workflows/clang-format-test.yml index 013ee47..9213b80 100644 --- a/.github/workflows/clang-format-test.yml +++ b/.github/workflows/clang-format-test.yml @@ -9,7 +9,7 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v6 - uses: DoozyX/clang-format-lint-action@v0.18.1 with: source: '.' From 3a162be230a0a7db3db9bf9c9aabef71f6e9f00e Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 26 Jun 2026 14:17:45 +1000 Subject: [PATCH 2/4] Feature/trace (#25) * jps trace added * updated how trace renders successors for JPS --- apps/jps.cpp | 122 ++++++++++++++++++++++++----- cmake/headers.cmake | 2 + extern/warthog-core | 2 +- include/jps/io/octile_grid_trace.h | 108 +++++++++++++++++++++++++ 4 files changed, 214 insertions(+), 20 deletions(-) create mode 100644 include/jps/io/octile_grid_trace.h 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 From 78e586363ad5b63366919bfa44012db46ee76e53 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 7 Aug 2026 11:19:44 +1000 Subject: [PATCH 3/4] Feature/dynamic scen (#27) * jps to work with dynamic environments * update to new warthog-core release v0.6.0 Reviewed by @dharabor --- .clang-format | 2 +- .github/workflows/clang-format-apply.yml | 5 +- .github/workflows/clang-format-test.yml | 5 +- CMakeLists.txt | 4 +- apps/jps.cpp | 420 ++++++++++++++---- extern/warthog-core | 2 +- include/jps/domain/rotate_gridmap.h | 54 +++ include/jps/search/jps_expansion_policy.h | 9 +- .../jps/search/jps_prune_expansion_policy.h | 10 +- 9 files changed, 397 insertions(+), 114 deletions(-) diff --git a/.clang-format b/.clang-format index b05423f..1d1d724 100644 --- a/.clang-format +++ b/.clang-format @@ -1,6 +1,6 @@ --- Language: Cpp -Standard: c++20 +Standard: Latest BasedOnStyle: GNU AccessModifierOffset: -4 PointerAlignment: Left diff --git a/.github/workflows/clang-format-apply.yml b/.github/workflows/clang-format-apply.yml index 4c9a158..77c173f 100644 --- a/.github/workflows/clang-format-apply.yml +++ b/.github/workflows/clang-format-apply.yml @@ -10,11 +10,12 @@ jobs: steps: - uses: actions/checkout@v6 - - uses: DoozyX/clang-format-lint-action@v0.18.1 + - uses: DoozyX/clang-format-lint-action@v0.20 with: source: '.' exclude: './extern ./libs' - clangFormatVersion: 18.1.3 + extensions: 'h,cpp,c' + clangFormatVersion: 20 inplace: True - run: | git config --local user.email "41898282+github-actions[bot]@users.noreply.github.com" diff --git a/.github/workflows/clang-format-test.yml b/.github/workflows/clang-format-test.yml index 9213b80..2a9d327 100644 --- a/.github/workflows/clang-format-test.yml +++ b/.github/workflows/clang-format-test.yml @@ -10,8 +10,9 @@ jobs: steps: - uses: actions/checkout@v6 - - uses: DoozyX/clang-format-lint-action@v0.18.1 + - uses: DoozyX/clang-format-lint-action@v0.20 with: source: '.' exclude: './extern ./libs' - clangFormatVersion: 18.1.3 + extensions: 'h,cpp,c' + clangFormatVersion: 20 diff --git a/CMakeLists.txt b/CMakeLists.txt index dea0e84..d56f483 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,11 +6,11 @@ project(WarthogJPS set_property(GLOBAL PROPERTY WARTHOG_warthog-jps ON) -set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD 23) set(CMAKE_CXX_STANDARD_REQUIRED TRUE) include(cmake/warthog.cmake) -warthog_module_declare(warthog-core v0.5.0) +warthog_module_declare(warthog-core v0.6.0) warthog_module(warthog-core) add_library(warthog_libjps) diff --git a/apps/jps.cpp b/apps/jps.cpp index 5448d47..3e034e4 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -1,4 +1,4 @@ -// warthog.cpp +// jps.cpp // // Pulls together a variety of different algorithms // for pathfinding on grid graphs. @@ -7,13 +7,19 @@ // @created: 2016-11-23 // +#include + #include #include #include #include + +#include +#include +#include #include #include -#include +#include #include #ifdef WARTHOG_POSTHOC #include @@ -26,7 +32,6 @@ #include "cfg.h" #include -#include #include #include @@ -38,8 +43,6 @@ #include #include -// #include "time_constraints.h" - namespace { // check computed solutions are optimal @@ -48,12 +51,15 @@ int checkopt = 0; int verbose = 0; // display program help on startup int print_help = 0; -// run only this query, or -1 for all +// run only this snapshot, or -1 for all +int snapshot_id = -1; +// run only this inst, or -1 for all int filter_id = -1; +std::string dump_map; #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_grid = ::warthog::io::grid_trace; using listener_type = std::tuple; #else using listener_type = std::tuple<>; @@ -62,7 +68,7 @@ using listener_type = std::tuple<>; void help(std::ostream& out) { - out << "warthog version " << WARTHOG_VERSION << "\n"; + out << "warthog version " WARTHOG_VERSION "\n"; out << "==> manual <==\n" << "This program solves/generates grid-based pathfinding " "problems using the\n" @@ -74,53 +80,52 @@ help(std::ostream& out) << "\t--scen [scen file] (required) \n" << "\t--map [map file] (optional; specify this to override map " "values in scen file) \n" - << "\t--costs [costs file] (required if using a weighted " - "terrain algorithm)\n" + << "\t--cost [type] (default first cost from scenario if exists;\n" + "\t\tuse cost type for solution from scenario or error if not " + "exists\n" + << "\t\tpass '-' to discard all provided solution costs)\n" << "\t--checkopt (optional; compare solution costs against " "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" + << "\t--snapshot [id] (default -1; select all instances (-1) or only " + "instances on snapshot id)\n" + << "\t--filter [num] (default -1; run all instances (-1) or run " + "instance num;\n" + << "\t\tif used with --snapshot, is instance num in snapshot id)\n" + << "\t--dump-map [file] (optional; dump gridmap at first instance to " + "[file], use with --snapshot or --filter)\n" #ifdef WARTHOG_POSTHOC << "\t--trace [.trace.yaml file] (optional; write posthoc trace for " - "first query to [file])\n" + "first instance 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" << "\tjps, jpsP or jps2, jps+, jpsP+ or jps2+\n"; - // << "" - // << "The following are valid parameters for GENERATING instances:\n" - // << "\t --gen [map file (required)]\n" - // << "Invoking the program this way generates at random 1000 valid - // problems for \n" - // << "gridmap [map file]\n"; } bool check_optimality( - warthog::search::solution& sol, warthog::util::experiment* exp) + const warthog::search::solution& sol, + const warthog::scenario::experiment* exp) { - uint32_t precision = 2; - double epsilon = (1.0 / (int)pow(10, precision)) / 2; - double delta = fabs(sol.sum_of_edge_costs_ - exp->distance()); - - if(fabs(delta - epsilon) > epsilon) + if(!exp->distance()) { - 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(); + // unknown solution + return true; + } + constexpr int32_t precision = 2; + double epsilon = std::pow(10.0, -precision) * 0.5; + double delta = std::fabs(sol.sum_of_edge_costs_ - *exp->distance()); - std::cerr << std::setprecision(exp->precision()); + if(delta > epsilon) + { std::cerr << "optimality check failed!" << std::endl; std::cerr << std::endl; - std::cerr << "optimal path length: " << exp->distance() + std::cerr << "optimal path length: " << sol.sum_of_edge_costs_ << " computed length: "; - std::cerr << sol.sum_of_edge_costs_ << std::endl; + std::cerr << *exp->distance() << std::endl; std::cerr << "precision: " << precision << " epsilon: " << epsilon << std::endl; std::cerr << "delta: " << delta << std::endl; @@ -135,44 +140,213 @@ check_optimality( #define WARTHOG_POSTHOC_DO(f) #endif +/// @brief general wrapper around scenario runner and gridmap management +/// +/// Gets given a scenario manager and handles program management of running +/// with user-provided parameters and algorithm support. +/// +/// Owns and update the gridmap over a dynamic scenario for algorithms that +/// support dynamic scenarios. +struct gridmap_scenario +{ + bool scenario_v1 = true; ///< scenario is v1 + bool grid_managed = false; ///< grid is managed by this class + bool static_scenario = false; ///< scenario is static + const warthog::scenario::scenario_manager* mgr; + warthog::scenario::scenario_runner run; + warthog::domain::gridmap grid; + jps::domain::rotate_gridmap rgrid; + warthog::scenario::grid_patch_set patches; + + gridmap_scenario(const warthog::scenario::scenario_manager& scen) + : mgr(&scen), run(&scen) + { + scenario_v1 + = mgr->get_version() == warthog::io::scenario_version::VERSION_1; + static_scenario = mgr->is_static_scenario(); + } + + /// @brief loads the map to current state + /// @param file map filename (single or patches) + /// @return true on success, false otherwise + bool + load_map(const std::filesystem::path file) + { + grid_managed = true; + if(!patches.load(file)) { return false; } + if(!run.gridmap_init(grid, patches)) { return false; } + return true; + } + + /// @brief will update to runner based on user-provided parameters + /// @param snapshot_id set map to match snapshot + /// @param filter_id if snapshot_id==-1, set map to match at instance id + /// @return true on success, false otherwise + bool + setup_runner(int snapshot_id, int filter_id) + { + if(snapshot_id != -1) + { + // goto snapshot_id + static_scenario = true; + while(run.get_snapshot_at() != snapshot_id) + { + // ran out of snapshots + if(run.complete()) + { + WARTHOG_GWARN_FMT( + "scenario complete before reaching snapshot {}", + snapshot_id); + return false; + } + // apply snapshot + run.snapshot_next(true); + run.snapshot_patches(false); + if(grid_managed) + { + // apply patches to inital grid + if(int c = run.gridmap_apply_patches(grid, patches); c < 0) + { + c = -c - 1; + auto p = run.get_patches()[c]; + WARTHOG_GWARN_FMT( + "failed to apply patch {} at ({},{})", p.patch_id, + p.topleft_x, p.topleft_y); + return false; + } + } + } + } + + if(filter_id != -1) + { + // skip next filter_id instances + static_scenario = true; + auto [exp, patches] = run.experiment_next(filter_id + 1, false); + if(run.complete() || exp == nullptr) + { + WARTHOG_GWARN_FMT( + "scenario complete before reaching filter {}", filter_id); + return false; + } + if(snapshot_id != -1 && run.get_snapshot_at() != snapshot_id) + { + WARTHOG_GWARN_FMT( + "scenario filter {} exceeded snapshot {} instances", + filter_id, snapshot_id); + return false; + } + } + + // update gridmap to match patch, as rgrid is not created, do through + // run + if(int c = run.gridmap_apply_patches(grid, patches); c < 0) + { + c = -c - 1; + auto p = run.get_patches()[c]; + WARTHOG_GWARN_FMT( + "failed to apply patch {} at ({},{})", p.patch_id, p.topleft_x, + p.topleft_y); + return false; + } + + rgrid.create_rmap(grid); + + return true; + } + + /// @brief apply patches from runner to owned grids (if managed) + /// @return true on success, false otherwise + bool + apply_patches() + { + if(!grid_managed) return true; + + // update grid through rgrid interface + for(auto& P : run.get_patches()) + { + uint32_t x, y; + grid.to_padded_xy_from_unpadded(P.topleft_x, P.topleft_y, x, y); + if(!rgrid.apply_patch_map( + patches.get_patch(P.patch_id), warthog::grid::point(x, y))) + { + return false; + } + if(!rgrid.apply_patch_rmap( + patches.get_patch(P.patch_id), warthog::grid::point(x, y))) + { + return false; + } + } + + return true; + } +}; + template int run_experiments( - Search& algo, std::string alg_name, - warthog::util::scenario_manager& scenmgr, bool verbose, bool checkopt, - std::ostream& out) + Search& algo, std::string alg_name, gridmap_scenario& scen, 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; + if(expander == nullptr) return (int)std::errc::invalid_argument; - out << "id\talg\texpanded\tgenerated\treopen\tsurplus\theapops" + out << "id\tsnapshot\talg\texpanded\tgenerated\treopen\tsurplus\theapops" << "\tnanos\tplen\tpcost\tscost\tmap\n"; - 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++) + + for(uint32_t i = 0;; ++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>) + +#endif + auto [exp, patch_count] = scen.run.experiment_next(); + if(exp == nullptr) { break; } + // check if only run one instance + if(filter_id >= 0 && i != 0) { break; } + // check if instance is on snapshot + if(snapshot_id >= 0 && scen.run.get_snapshot_at() != snapshot_id) { - if(i == filter_id && !trace_file.empty()) + break; + } + + if(patch_count != 0) + { + if(!scen.apply_patches()) { - listener_grid& l - = std::get(algo.get_listeners()); - trace_stream.emplace(trace_file); - l.open(*trace_stream); + // failed to apply patches, exit + WARTHOG_GCRIT("dynamic patch error: failed to apply patches"); + return (int)std::errc::io_error; } } + + // special actions on first scenario + if(i == 0) + { + // print map + if(!dump_map.empty()) { scen.grid.save(dump_map, false); } + // trace +#ifdef WARTHOG_POSTHOC + if constexpr(std::same_as< + listener_type, + std::remove_cvref_t< + decltype(algo.get_listeners())>>) + { + if(!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 = expander->get_pack(exp->startx(), exp->starty()); @@ -182,6 +356,11 @@ run_experiments( sol.reset(); algo.get_path(&pi, &par, &sol); + // check for no solution + if(sol.sum_of_edge_costs_ >= warthog::COST_MAX) + { + sol.sum_of_edge_costs_ = -1; + } #ifdef WARTHOG_POSTHOC if constexpr(std::same_as< @@ -196,46 +375,77 @@ run_experiments( } #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_ - << "\t" << sol.met_.heap_ops_ << "\t" + out << scen.run.get_experiment_at() << "\t" + << scen.run.get_snapshot_at() << "\t" << alg_name << "\t" + << sol.met_.nodes_expanded_ << "\t" << sol.met_.nodes_generated_ + << "\t" << sol.met_.nodes_reopen_ << "\t" + << sol.met_.nodes_surplus_ << "\t" << sol.met_.heap_ops_ << "\t" << sol.met_.time_elapsed_nano_.count() << "\t" << (!sol.path_.empty() ? sol.path_.size() - 1 : 0) << "\t" - << sol.sum_of_edge_costs_ << "\t" << exp->distance() << "\t" - << scenmgr.last_file_loaded() << std::endl; + << sol.sum_of_edge_costs_ << "\t"; + if(exp->distance()) + out << *exp->distance(); + else + out << '-'; + out << "\t" << scen.mgr->last_file_loaded() << std::endl; if(checkopt) { if(!check_optimality(sol, exp)) { WARTHOG_GCRIT("search error: failed suboptimal 4"); - return 4; + return (int)std::errc::result_out_of_range; } } } WARTHOG_GINFO_FMT( - "search complete; total memory: {}", algo.mem() + scenmgr.mem()); + "search complete; total memory: {}", algo.mem() + scen.mgr->mem()); return 0; } -template +template int run_jps( - warthog::util::scenario_manager& scenmgr, std::string mapname, + warthog::scenario::scenario_manager& scenmgr, std::string mapname, std::string alg_name) { - warthog::domain::gridmap map(mapname.c_str()); - ExpansionPolicy expander(&map); - warthog::heuristic::octile_heuristic heuristic(map.width(), map.height()); + gridmap_scenario scen(scenmgr); + if(!scen.load_map(std::filesystem::path(mapname))) + { + WARTHOG_GCRIT("failed to load map"); + return (int)std::errc::io_error; + } + // init runner to start at correct instance and update the map + if(!scen.setup_runner(snapshot_id, filter_id)) + { + WARTHOG_GCRIT("failed to setup scenario"); + return (int)std::errc::io_error; + } + if constexpr(!Online) + { + // check that scenario is offline + if(!scen.static_scenario) + { + WARTHOG_GCRIT_FMT( + "algorithm {} requires scenario file/filter/snapshot to be " + "static (restrict to single snapshot)", + alg_name); + return (int)std::errc::invalid_argument; + } + } + ExpansionPolicy expander(nullptr); + expander.set_map(scen.rgrid); // gridmap_scenario manages both grids + warthog::heuristic::octile_heuristic heuristic( + scen.grid.width(), scen.grid.height()); warthog::util::pqueue_min open; warthog::search::unidirectional_search jps( - &heuristic, &expander, &open, listener_type(WARTHOG_POSTHOC_DO(&map))); + &heuristic, &expander, &open, + listener_type(WARTHOG_POSTHOC_DO(&scen.grid))); - int ret = run_experiments( - jps, alg_name, scenmgr, verbose, checkopt, std::cout); + int ret + = run_experiments(jps, alg_name, scen, verbose, checkopt, std::cout); return ret; } @@ -252,12 +462,14 @@ main(int argc, char** argv) // {"gen", required_argument, 0, 3}, {"help", no_argument, &print_help, 1}, {"checkopt", no_argument, &checkopt, 1}, + {"cost", required_argument, 0, 0}, {"verbose", no_argument, &verbose, 1}, + {"snapshot", required_argument, &snapshot_id, 1}, {"filter", required_argument, &filter_id, 1}, + {"dump-map", required_argument, 0, 0}, #ifdef WARTHOG_POSTHOC {"trace", required_argument, 0, 0}, #endif - {"costs", required_argument, 0, 1}, {0, 0, 0, 0}}; warthog::util::cfg cfg; @@ -272,26 +484,34 @@ main(int argc, char** argv) std::string sfile = cfg.get_param_value("scen"); std::string alg = cfg.get_param_value("alg"); // std::string gen = cfg.get_param_value("gen"); - std::string mapfile = cfg.get_param_value("map"); - std::string costfile = cfg.get_param_value("costs"); + std::string mapfile = cfg.get_param_value("map"); + std::string costtype = cfg.get_param_value("cost"); + std::string weightsfile = cfg.get_param_value("grid-weight"); + dump_map = cfg.get_param_value("dump-map"); + if(snapshot_id == 1) + { + if(warthog::util::parse_token( + cfg.get_param_value("snapshot"), snapshot_id) + != std::errc{}) + { + WARTHOG_GERROR_FMT("invalid --snapshot argument {}", snapshot_id); + return (int)std::errc::invalid_argument; + } + } if(filter_id == 1) { - filter_id = std::stoi(cfg.get_param_value("filter")); + if(warthog::util::parse_token(cfg.get_param_value("filter"), filter_id) + != std::errc{}) + { + WARTHOG_GERROR_FMT("invalid --filter argument {}", filter_id); + return (int)std::errc::invalid_argument; + } } #ifdef WARTHOG_POSTHOC trace_file = cfg.get_param_value("trace"); #endif - // if(gen != "") - // { - // warthog::util::scenario_manager sm; - // warthog::domain::gridmap gm(gen.c_str()); - // sm.generate_experiments(&gm, 1000) ; - // sm.write_scenario(std::cout); - // exit(0); - // } - // running experiments if(alg == "" || sfile == "") { @@ -300,59 +520,63 @@ main(int argc, char** argv) } // load up the instances - warthog::util::scenario_manager scenmgr; - scenmgr.load_scenario(sfile.c_str()); + warthog::scenario::scenario_manager scenmgr; + scenmgr.set_cost_type(costtype); + try + { + scenmgr.load_scenario(sfile.c_str()); + } + catch(const std::runtime_error& e) + { + return (int)std::errc::io_error; + } if(scenmgr.num_experiments() == 0) { - std::cerr << "err; scenario file does not contain any instances\n"; - return 1; + WARTHOG_GCRIT("scenario file does not contain any instances"); + return (int)std::errc::invalid_argument; } // 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); + mapfile = warthog::scenario::find_map_filename(scenmgr, sfile); if(mapfile.empty()) { std::cerr << "could not locate a corresponding map file\n"; help(std::cout); return 0; } + WARTHOG_GINFO_FMT("deduced mapfile: ", mapfile); } - std::cerr << "mapfile=" << mapfile << std::endl; using namespace jps::search; if(alg == "jps") { using jump_point = jps::jump::jump_point_online; - return run_jps>( + return run_jps, true>( scenmgr, mapfile, alg); } else if(alg == "jpsP" || alg == "jps2") { using jump_point = jps::jump::jump_point_online; - return run_jps>( + return run_jps, true>( scenmgr, mapfile, alg); } else if(alg == "jps+") { using jump_point = jps::jump::jump_point_offline<>; - return run_jps>( + return run_jps, false>( scenmgr, mapfile, alg); } else if(alg == "jpsP+" || alg == "jps2+") { using jump_point = jps::jump::jump_point_offline<>; - return run_jps>( + return run_jps, false>( scenmgr, mapfile, alg); } - else - { - std::cerr << "err; invalid search algorithm: " << alg << "\n"; - return 1; - } - return 0; + WARTHOG_GCRIT_FMT("invalid search algorithm: ", alg); + return (int)std::errc::invalid_argument; } diff --git a/extern/warthog-core b/extern/warthog-core index ba9b5f6..1866931 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit ba9b5f66281ca6a767360787037ae80a7226e38e +Subproject commit 18669316c6621376d6a469795968b97e635c65e3 diff --git a/include/jps/domain/rotate_gridmap.h b/include/jps/domain/rotate_gridmap.h index ae4688b..0c41d3e 100644 --- a/include/jps/domain/rotate_gridmap.h +++ b/include/jps/domain/rotate_gridmap.h @@ -521,6 +521,7 @@ class rotate_gridmap : public rgridmap_point_conversions } else { clear(); } } + void clear() { @@ -528,6 +529,7 @@ class rotate_gridmap : public rgridmap_point_conversions maps = {}; static_cast(*this) = {}; } + void create_rmap(domain::gridmap& map) { @@ -571,6 +573,58 @@ class rotate_gridmap : public rgridmap_point_conversions #endif // NDEBUG } + bool + apply_patch_map(domain::gridmap::bittable patch, point padded_loc) + { + if((uint64_t)padded_loc.x + patch.width() >= (uint64_t)map().width() + || (uint64_t)padded_loc.y + patch.height() + >= (uint64_t)map().height()) + return false; + + // apply patch + patch.copy( + map(), point_to_id(padded_loc), pad_id::zero(), patch.width(), + patch.height()); + + return true; + } + + bool + apply_patch_rmap(domain::gridmap::bittable patch, point padded_loc) + { + if((uint64_t)padded_loc.x + patch.width() >= (uint64_t)map().width() + || (uint64_t)padded_loc.y + patch.height() + >= (uint64_t)map().height()) + return false; + + // apply patch + // copy row-by-row on rmap, thus start at bottom-right corner of patch + rgrid_id dest_id = rpoint_to_rid(point_to_rpoint( + point(padded_loc.x, padded_loc.y + patch.height() - 1))); + grid_id src_id + = static_cast(patch.xy_to_id(0, patch.height() - 1)); + const uint32_t dest_y_adj = rmap().width(); + auto& rgridp = rmap(); + + // go left-to-right, bottom-to-top on patch + for(uint32_t x = 0; x < patch.width(); x++) + { + auto row_dest_id = dest_id; + dest_id.id += dest_y_adj; + auto row_src_id = src_id; + src_id.id += 1; + for(uint32_t y = 0; y < patch.height(); y++) + { + bool label = patch.get(static_cast(row_src_id)); + row_src_id.id -= patch.width(); + rgridp.set_label(static_cast(row_dest_id), label); + row_dest_id.id += 1; + } + } + + return true; + } + domain::gridmap& map() noexcept { diff --git a/include/jps/search/jps_expansion_policy.h b/include/jps/search/jps_expansion_policy.h index 39714bf..4d7acbf 100644 --- a/include/jps/search/jps_expansion_policy.h +++ b/include/jps/search/jps_expansion_policy.h @@ -184,10 +184,11 @@ jps_expansion_policy::expand( if(jump_result.first > 0) // jump point { // successful jump - pad_id node{pad_id(static_cast( - current_id.id - + warthog::grid::dir_id_adj(di, map_width_) - * res.inter))}; + pad_id node{pad_id( + static_cast( + current_id.id + + warthog::grid::dir_id_adj(di, map_width_) + * res.inter))}; assert(rmap_.map().get( node)); // successor must be traversable warthog::search::search_node* jp_succ diff --git a/include/jps/search/jps_prune_expansion_policy.h b/include/jps/search/jps_prune_expansion_policy.h index 2af154f..66dc2f8 100644 --- a/include/jps/search/jps_prune_expansion_policy.h +++ b/include/jps/search/jps_prune_expansion_policy.h @@ -257,8 +257,9 @@ jps_prune_expansion_policy::expand( * res_i.hori); const auto cost_j = cost + warthog::DBL_ONE * res_i.hori; - assert(rmap_.map().get(pad_id{ - node_j})); // successor must be traversable + assert(rmap_.map().get( + pad_id{ + node_j})); // successor must be traversable warthog::search::search_node* jp_succ = this->generate(pad_id{node_j}); add_neighbour(jp_succ, cost_j); @@ -271,8 +272,9 @@ jps_prune_expansion_policy::expand( * res_i.vert); const auto cost_j = cost + warthog::DBL_ONE * res_i.vert; - assert(rmap_.map().get(pad_id{ - node_j})); // successor must be traversable + assert(rmap_.map().get( + pad_id{ + node_j})); // successor must be traversable warthog::search::search_node* jp_succ = this->generate(pad_id{node_j}); add_neighbour(jp_succ, cost_j); From b3e82444e833ffd89de9839164fec8479f179cff Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 7 Aug 2026 11:23:30 +1000 Subject: [PATCH 4/4] update version to 0.6.0 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d56f483..473e5b1 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.6.0 LANGUAGES CXX C) set_property(GLOBAL PROPERTY WARTHOG_warthog-jps ON)