diff --git a/apps/warthog.cpp b/apps/warthog.cpp index c8a420a..50ab3ca 100644 --- a/apps/warthog.cpp +++ b/apps/warthog.cpp @@ -13,6 +13,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -48,8 +51,11 @@ 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 inst, or -1 for all int filter_id = -1; +// dump map at id if set +int dump_map_id = -1; +std::string dump_map_file; #ifdef WARTHOG_POSTHOC // write trace to file, empty string to disable std::string trace_file; @@ -76,24 +82,33 @@ help(std::ostream& out) "values in scen file) \n" << "\t--costs [costs file] (required if using a weighted " "terrain algorithm)\n" + << "\t--v2-cost [type] (optional; change used cost type for v2 scen " + "file dynamic)" << "\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--filter [id] (optional; run only inst [id])\n" + << "\t--dump-map [id] (optional; dump map at id to stderr)" + << "\t--dump-map-file [filename] (optional; file to dump map to, " + "default /dev/stderr)" #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" - << "\tastar, astar_wgm, astar4c, dijkstra\n"; + << "\tastar, astar_wgm, astar4c, dijkstra\n" + << "Currently recognised values for [v2-cost]:\n" + << "8c-ncc (default), 8c-cc, 4c, aa-ncc, aa-cc\n" + << "8c = 8-connected, 4c = 4-connected, aa = anyangle, ncc = " + "no-corner-cut, cc = corner-cut\n"; } bool check_optimality( - warthog::search::solution& sol, warthog::util::experiment* exp) + const warthog::search::solution& sol, const warthog::util::experiment* exp) { uint32_t precision = 2; double epsilon = (1.0 / (int)pow(10, precision)) / 2; @@ -101,20 +116,12 @@ check_optimality( if(fabs(delta - epsilon) > epsilon) { - 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: " << stroptlen.str() + std::cerr << "optimal path length: " << sol.sum_of_edge_costs_ << " computed length: "; - std::cerr << strpathlen.str() << std::endl; + std::cerr << exp->distance() << std::endl; std::cerr << "precision: " << precision << " epsilon: " << epsilon << std::endl; std::cerr << "delta: " << delta << std::endl; @@ -129,12 +136,30 @@ check_optimality( #define WARTHOG_POSTHOC_DO(f) #endif +struct gridmap_scenario +{ + const warthog::manager::scenario_manager* mgr; + warthog::manager::scenario_runner run; + warthog::domain::gridmap grid; + warthog::manager::grid_patch_set patches; + + gridmap_scenario(const warthog::manager::scenario_manager& scen) + : mgr(&scen), run(&scen) + { } + + bool + load_map(const std::filesystem::path map) + { + if(!patches.load(map)) { return false; } + return run.gridmap_init(grid, patches); + } +}; + 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; @@ -142,31 +167,83 @@ run_experiments( auto* expander = algo.get_expander(); if(expander == nullptr) return 1; - 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; } + if(patch_count != 0) + { + if(scen.run.gridmap_apply_patches(scen.grid, scen.patches) < 0) + { + // failed to apply patches, exit + WARTHOG_GCRIT("dynamic patch error: failed to apply patches"); + return 5; + } + } + + if(i == dump_map_id) { - if(i == filter_id && !trace_file.empty()) + // print map + std::optional outstream; + std::ostream* out = nullptr; + // convert dev to stream for cross-platform support + if(dump_map_file == "/dev/stderr") { out = &std::cerr; } + else if(dump_map_file == "/dev/stdout") { out = &std::cout; } + else + { + out = &outstream.emplace(dump_map_file); + WARTHOG_GERROR_FMT_IF( + !*out, "failed to open file to dump map {}\n", + dump_map_file); + } + if(*out) { - listener_grid& l - = std::get(algo.get_listeners()); - trace_stream.emplace(trace_file); - l.open(*trace_stream); + // out is valid, print + std::string line(scen.grid.width() + 1, '\n'); + for(uint32_t y = 0, ye = scen.grid.height(), + xe = scen.grid.width(); + y < ye; ++y) + { + for(uint32_t x = 0; x < xe; ++x) + { + line[x] = "@."[( + int)(scen.grid.get_label( + scen.grid.to_padded_id_from_padded(x, y)) + != 0)]; + } + *out << line; + } } } + + if(filter_id >= 0 && i == filter_id) + { + // 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); + } + else if(filter_id >= 0) { continue; } warthog::pack_id startid = expander->get_pack(exp->startx(), exp->starty()); @@ -176,6 +253,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< @@ -190,14 +272,14 @@ 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" - << sol.met_.time_elapsed_nano_.count() << "\t" + out << i << "\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; + << scen.mgr->last_file_loaded() << std::endl; if(checkopt) { @@ -210,7 +292,7 @@ run_experiments( } WARTHOG_GINFO_FMT( - "search complete; total memory: {}", algo.mem() + scenmgr.mem()); + "search complete; total memory: {}", algo.mem() + scen.mgr->mem()); return 0; } @@ -219,16 +301,23 @@ run_astar( warthog::util::scenario_manager& scenmgr, std::string mapname, std::string alg_name) { - warthog::domain::gridmap map(mapname.c_str()); - warthog::search::gridmap_expansion_policy 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 3; + } + warthog::search::gridmap_expansion_policy expander(&scen.grid); + warthog::heuristic::octile_heuristic heuristic( + scen.grid.width(), scen.grid.height()); warthog::util::pqueue_min open; warthog::search::unidirectional_search astar( - &heuristic, &expander, &open, listener_type(WARTHOG_POSTHOC_DO(&map))); + &heuristic, &expander, &open, + listener_type(WARTHOG_POSTHOC_DO(&scen.grid))); - int ret = run_experiments( - astar, alg_name, scenmgr, verbose, checkopt, std::cout); + int ret + = run_experiments(astar, alg_name, scen, verbose, checkopt, std::cout); return ret; } @@ -237,17 +326,23 @@ run_astar4c( warthog::util::scenario_manager& scenmgr, std::string mapname, std::string alg_name) { - warthog::domain::gridmap map(mapname.c_str()); - warthog::search::gridmap_expansion_policy expander(&map, true); + gridmap_scenario scen(scenmgr); + if(!scen.load_map(std::filesystem::path(mapname))) + { + WARTHOG_GCRIT("failed to load map"); + return 3; + } + warthog::search::gridmap_expansion_policy expander(&scen.grid, true); warthog::heuristic::manhattan_heuristic heuristic( - map.width(), map.height()); + scen.grid.width(), scen.grid.height()); warthog::util::pqueue_min open; warthog::search::unidirectional_search astar( - &heuristic, &expander, &open, listener_type(WARTHOG_POSTHOC_DO(&map))); + &heuristic, &expander, &open, + listener_type(WARTHOG_POSTHOC_DO(&scen.grid))); - int ret = run_experiments( - astar, alg_name, scenmgr, verbose, checkopt, std::cout); + int ret + = run_experiments(astar, alg_name, scen, verbose, checkopt, std::cout); return ret; } @@ -256,16 +351,22 @@ run_dijkstra( warthog::util::scenario_manager& scenmgr, std::string mapname, std::string alg_name) { - warthog::domain::gridmap map(mapname.c_str()); - warthog::search::gridmap_expansion_policy expander(&map); + gridmap_scenario scen(scenmgr); + if(!scen.load_map(std::filesystem::path(mapname))) + { + WARTHOG_GCRIT("failed to load map"); + return 3; + } + warthog::search::gridmap_expansion_policy expander(&scen.grid); warthog::heuristic::zero_heuristic heuristic; warthog::util::pqueue_min open; warthog::search::unidirectional_search astar( - &heuristic, &expander, &open, listener_type(WARTHOG_POSTHOC_DO(&map))); + &heuristic, &expander, &open, + listener_type(WARTHOG_POSTHOC_DO(&scen.grid))); - int ret = run_experiments( - astar, alg_name, scenmgr, verbose, checkopt, std::cout); + int ret + = run_experiments(astar, alg_name, scen, verbose, checkopt, std::cout); return ret; } @@ -274,6 +375,8 @@ run_wgm_astar( warthog::util::scenario_manager& scenmgr, std::string mapname, std::string alg_name, std::string costfile) { + gridmap_scenario scen(scenmgr); + // do not load map here warthog::util::cost_table costs(costfile.c_str()); warthog::domain::vl_gridmap map(mapname.c_str()); warthog::search::vl_gridmap_expansion_policy expander(&map, costs); @@ -291,8 +394,8 @@ run_wgm_astar( warthog::search::unidirectional_search astar(&heuristic, &expander, &open); - int ret = run_experiments( - astar, alg_name, scenmgr, verbose, checkopt, std::cout); + int ret + = run_experiments(astar, alg_name, scen, verbose, checkopt, std::cout); return ret; } @@ -311,10 +414,13 @@ main(int argc, char** argv) {"checkopt", no_argument, &checkopt, 1}, {"verbose", no_argument, &verbose, 1}, {"filter", required_argument, &filter_id, 1}, + {"dump-map", required_argument, &dump_map_id, 1}, + {"dump-map-file", required_argument, 0, 0}, #ifdef WARTHOG_POSTHOC {"trace", required_argument, 0, 0}, #endif - {"costs", required_argument, 0, 1}, + {"costs", required_argument, 0, 0}, + {"v2-cost", required_argument, 0, 0}, {0, 0, 0, 0}}; warthog::util::cfg cfg; @@ -331,6 +437,8 @@ main(int argc, char** argv) // 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 v2cost = cfg.get_param_value("v2-cost"); + dump_map_file = cfg.get_param_value("dump-map-file"); if(filter_id == 1) { @@ -340,15 +448,6 @@ main(int argc, char** argv) 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 == "") { @@ -356,8 +455,18 @@ main(int argc, char** argv) return 0; } + // check v2cost + if(v2cost.empty()) { v2cost = "8c-ncc"; } + + if(dump_map_id == 1) + { + dump_map_id = std::stoi(cfg.get_param_value("dump-map")); + } + if(dump_map_file.empty()) { dump_map_file = "/dev/stderr"; } + // load up the instances warthog::util::scenario_manager scenmgr; + scenmgr.set_cost_type(v2cost); scenmgr.load_scenario(sfile.c_str()); if(scenmgr.num_experiments() == 0) diff --git a/cmake/headers.cmake b/cmake/headers.cmake index 46e1ba6..863f2cb 100644 --- a/cmake/headers.cmake +++ b/cmake/headers.cmake @@ -26,8 +26,20 @@ include/warthog/heuristic/manhattan_heuristic.h include/warthog/heuristic/octile_heuristic.h include/warthog/heuristic/zero_heuristic.h +include/warthog/io/fwd.h include/warthog/io/grid.h +include/warthog/io/grid_trace.h include/warthog/io/log.h +include/warthog/io/observer.h +include/warthog/io/posthoc_trace.h +include/warthog/io/scenario.h +include/warthog/io/serialize_base.h +include/warthog/io/stream_observer.h + +include/warthog/manager/experiment.h +include/warthog/manager/grid_patch_set.h +include/warthog/manager/scenario_manager.h +include/warthog/manager/scenario_runner.h include/warthog/memory/arraylist.h include/warthog/memory/bittable.h @@ -60,6 +72,7 @@ include/warthog/util/intrin.h include/warthog/util/macros.h include/warthog/util/pqueue.h include/warthog/util/scenario_manager.h +include/warthog/util/string.h include/warthog/util/template.h include/warthog/util/timer.h include/warthog/util/vec_io.h diff --git a/include/warthog/domain/grid.h b/include/warthog/domain/grid.h index 558b670..455edac 100644 --- a/include/warthog/domain/grid.h +++ b/include/warthog/domain/grid.h @@ -1,13 +1,18 @@ #ifndef WARTHOG_DOMAIN_GRID_H #define WARTHOG_DOMAIN_GRID_H -// domains/grid.h -// -// A place for grid-related curios -// -// @author: dharabor -// @created: 2018-11-03 -// +/// @file domains/grid.h +/// +/// A place for grid-related features. +/// Includes the grid identifier (id), direction (and direction_id), +/// point (coordinate) and utilities. +/// +/// These utilities include conversion between direction/direction_id, +/// rotation, direction to point and inverse, helpers for grid traversal, etc. +/// +/// @author: dharabor & Ryan Hechenberger +/// @created: 2018-11-03 +/// #include #include @@ -20,8 +25,6 @@ namespace warthog::grid { -// TODO: document - using grid_id = pad32_id; typedef enum : uint8_t @@ -71,25 +74,25 @@ template concept SecInterCardinalID = static_cast(D) >= 8; /// @return is NORTH_ID/SOUTH_ID/EAST_ID/WEST_ID -constexpr inline bool +constexpr bool is_cardinal_id(direction_id d) noexcept { return static_cast(d) < 4; } /// @return is NORTHEAST_ID/NORTHWEST_ID/SOUTHEAST_ID/SOUTHWEST_ID -constexpr inline bool +constexpr bool is_intercardinal_id(direction_id d) noexcept { return static_cast(d - 4) < 4; } /// @return is a intercardinal+cardinal combined direction -constexpr inline bool +constexpr bool is_secic_id(direction_id d) noexcept { return static_cast(d) >= 8; } /// @return get the cardinal id from a combined direction -constexpr inline direction_id +constexpr direction_id secic_cardinal(direction_id d) noexcept { direction_id c @@ -98,7 +101,7 @@ secic_cardinal(direction_id d) noexcept return c; } /// @return get the intercardinal id from a combined direction -constexpr inline direction_id +constexpr direction_id secic_intercardinal(direction_id d) noexcept { direction_id c = static_cast(static_cast(d) >> 3); @@ -519,7 +522,7 @@ operator==(spoint a, spoint b) return std::bit_cast(a) == std::bit_cast(b); } -constexpr inline std::pair +constexpr std::pair point_signed_diff(point a, point b) noexcept { return { @@ -529,20 +532,20 @@ point_signed_diff(point a, point b) noexcept static_cast(b.y) - static_cast(a.y))}; } -constexpr inline point +constexpr point operator+(point a, spoint b) noexcept { return point{ static_cast(a.x + static_cast(b.x)), static_cast(a.y + static_cast(b.y))}; } -constexpr inline spoint +constexpr spoint operator+(spoint a, spoint b) noexcept { return spoint{ static_cast(a.x + b.x), static_cast(a.y + b.y)}; } -constexpr inline spoint +constexpr spoint operator*(int16_t a, spoint b) noexcept { return spoint{ @@ -552,7 +555,7 @@ operator*(int16_t a, spoint b) noexcept /// @brief gets a unit signed-point in direction /// @param d the direction for the unit-distance /// @return the unit spoint -constexpr inline spoint +constexpr spoint dir_unit_point(direction_id d) noexcept { assert(static_cast(d) < 8); @@ -582,7 +585,7 @@ dir_unit_point(direction_id d) noexcept /// @param d the direction for unit-point, if a secic will use the /// intercardinal component /// @return the unit spoint -constexpr inline spoint +constexpr spoint dir_unit_point_secic(direction_id d) noexcept { assert(static_cast(d) < 8); @@ -614,7 +617,7 @@ dir_unit_point_secic(direction_id d) noexcept /// @return the direction from p1 to p2. If diff x or diff y is zero, will be /// cardinal, otherwise is intercardinal direction. -constexpr inline direction_id +constexpr direction_id point_to_direction_id(point p1, point p2) noexcept { union diff --git a/include/warthog/domain/gridmap.h b/include/warthog/domain/gridmap.h index efeea4b..a986609 100644 --- a/include/warthog/domain/gridmap.h +++ b/include/warthog/domain/gridmap.h @@ -14,12 +14,13 @@ // in a one dimensional array and also to avoid range checks when trying to // identify invalid neighbours of tiles on the edge of the map. // -// @author: dharabor +// @author: dharabor & Ryan Hechenberger // @created: 08/08/2012 // #include "grid.h" #include +#include #include #include #include @@ -31,6 +32,8 @@ #include #include #include +#include +#include #include namespace warthog::domain @@ -44,7 +47,12 @@ class gridmap : public memory::bittable public: using bittable = gridmap::bittable; // inform of type existing using bitarray = gridmap::bitarray; // inform of type existing + gridmap(); gridmap(uint32_t height, uint32_t width); + gridmap(std::istream& input); + gridmap(io::bittable_serialize& parser); + gridmap(std::filesystem::path&& filename); + gridmap(const std::filesystem::path& filename); gridmap(const char* filename); gridmap(const gridmap&) = delete; ~gridmap(); @@ -56,8 +64,20 @@ class gridmap : public memory::bittable operator=(const gridmap&) = delete; - // here we convert from the coordinate space of - // the original grid to the coordinate space of db_. + void + setup(uint32_t height, uint32_t width); + void + load(std::istream& input); + void + load(io::bittable_serialize& parser); + void + load(std::filesystem::path&& filename); + void + load(const std::filesystem::path& filename); + void + load(const char* filename); + + /// @brief convert unpadded id to padded id pad_id to_padded_id(pack_id node_id) const noexcept { @@ -70,19 +90,20 @@ class gridmap : public memory::bittable (uint32_t{node_id} / header_.width_) * padding_per_row_}; } - // here we convert from the coordinate space of - // the original grid to the coordinate space of db_. + /// @brief convert unpadded (x,y) to a padded id pad_id to_padded_id_from_unpadded(uint32_t x, uint32_t y) const noexcept { return pad_id{(y + PADDED_ROWS) * width() + x}; } + /// @brief convert padded (x,y) to a padded id pad_id to_padded_id_from_padded(uint32_t x, uint32_t y) const noexcept { return pad_id{y * width() + x}; } + /// @brief convert unpadded id to unpadded (x,y) void to_unpadded_xy(pack_id grid_id, uint32_t& x, uint32_t& y) const noexcept { @@ -91,6 +112,7 @@ class gridmap : public memory::bittable assert(x < header_.width_ && y < header_.height_); } + /// @brief convert padded id to unpadded (x,y) void to_unpadded_xy(pad_id grid_id, uint32_t& x, uint32_t& y) const noexcept { @@ -99,6 +121,18 @@ class gridmap : public memory::bittable assert(x < header_.width_ && y < header_.height_); } + /// @brief convert padded (x,y) to unpadded (x,y) + void + to_unpadded_xy_from_padded( + uint32_t padded_x, uint32_t padded_y, uint32_t& x, + uint32_t& y) const noexcept + { + y = padded_y - PADDED_ROWS; + x = padded_x; + assert(x < header_.width_ && y < header_.height_); + } + + /// @brief convert padded id to padded (x,y) void to_padded_xy(pad_id grid_id, uint32_t& x, uint32_t& y) const noexcept { @@ -107,6 +141,18 @@ class gridmap : public memory::bittable assert(x < width() && y < height()); } + /// @brief convert unpadded (x,y) to padded (x,y) + void + to_padded_xy_from_unpadded( + uint32_t unpadded_x, uint32_t unpadded_y, uint32_t& x, + uint32_t& y) const noexcept + { + y = unpadded_y + PADDED_ROWS; + x = unpadded_x; + assert(x < width() && y < height()); + } + + /// @brief convert padded id to unpadded id pack_id to_unpadded_id(pad_id grid_id) const noexcept { @@ -119,6 +165,7 @@ class gridmap : public memory::bittable // as the padded width is already removed PADDED_ROWS * header_.width_}; } + /// @brief convert unpadded (x,y) to unpadded id pack_id to_unpadded_id_from_unpadded(uint32_t x, uint32_t y) const noexcept { @@ -150,12 +197,12 @@ class gridmap : public memory::bittable // read from the byte just before node_id and shift down until the // nei adjacent to node_id is in the lowest position - tiles[0] - = (uint8_t)(*((uint32_t*)(db_ + (pos1 - 1))) >> (bit_offset + 7)); - tiles[1] - = (uint8_t)(*((uint32_t*)(db_ + (pos2 - 1))) >> (bit_offset + 7)); - tiles[2] - = (uint8_t)(*((uint32_t*)(db_ + (pos3 - 1))) >> (bit_offset + 7)); + tiles[0] = (uint8_t)(*((uint32_t*)(db_.get() + (pos1 - 1))) + >> (bit_offset + 7)); + tiles[1] = (uint8_t)(*((uint32_t*)(db_.get() + (pos2 - 1))) + >> (bit_offset + 7)); + tiles[2] = (uint8_t)(*((uint32_t*)(db_.get() + (pos3 - 1))) + >> (bit_offset + 7)); } // takes the tiles from get_neighbours and tightly packs them into 8-bits @@ -207,9 +254,12 @@ class gridmap : public memory::bittable // read 32bits of memory; grid_id_p is in the // lowest bit position of tiles[1] - tiles[0] = (uint32_t)(*((uint64_t*)(db_ + pos1)) >> (bit_offset)); - tiles[1] = (uint32_t)(*((uint64_t*)(db_ + pos2)) >> (bit_offset)); - tiles[2] = (uint32_t)(*((uint64_t*)(db_ + pos3)) >> (bit_offset)); + tiles[0] + = (uint32_t)(*((uint64_t*)(db_.get() + pos1)) >> (bit_offset)); + tiles[1] + = (uint32_t)(*((uint64_t*)(db_.get() + pos2)) >> (bit_offset)); + tiles[2] + = (uint32_t)(*((uint64_t*)(db_.get() + pos3)) >> (bit_offset)); } // similar to get_neighbours_32bit but grid_id_p is placed into the @@ -238,9 +288,12 @@ class gridmap : public memory::bittable // read 32bits of memory; grid_id_p is in the // highest bit position of tiles[1] - tiles[0] = (uint32_t)(*((uint64_t*)(db_ + pos1)) >> (bit_offset + 1)); - tiles[1] = (uint32_t)(*((uint64_t*)(db_ + pos2)) >> (bit_offset + 1)); - tiles[2] = (uint64_t)(*((uint64_t*)(db_ + pos3)) >> (bit_offset + 1)); + tiles[0] + = (uint32_t)(*((uint64_t*)(db_.get() + pos1)) >> (bit_offset + 1)); + tiles[1] + = (uint32_t)(*((uint64_t*)(db_.get() + pos2)) >> (bit_offset + 1)); + tiles[2] + = (uint64_t)(*((uint64_t*)(db_.get() + pos3)) >> (bit_offset + 1)); } // fetches a contiguous set of tiles from three adjacent rows. @@ -259,9 +312,9 @@ class gridmap : public memory::bittable uint32_t pos3 = dbindex + dbwidth64_; // read 64bits of tile data from each of the three rows - tiles[0] = *((uint64_t*)(db_) + pos1); - tiles[1] = *((uint64_t*)(db_) + pos2); - tiles[2] = *((uint64_t*)(db_) + pos3); + tiles[0] = *((uint64_t*)(db_.get()) + pos1); + tiles[1] = *((uint64_t*)(db_.get()) + pos2); + tiles[2] = *((uint64_t*)(db_.get()) + pos3); } // fetches a contiguous set of tiles from three adjacent rows. @@ -327,7 +380,7 @@ class gridmap : public memory::bittable const char* filename() const noexcept { - return this->filename_; + return this->filename_.c_str(); } uint32_t @@ -348,21 +401,24 @@ class gridmap : public memory::bittable return sizeof(*this) + sizeof(warthog::dbword) * db_size_; } -private: - using bittable::setup; - - warthog::util::gm_header header_; - warthog::dbword* db_; - char filename_[256]; - - uint32_t dbwidth_; - uint32_t dbwidth64_; - uint32_t dbheight_; - uint32_t db_size_; - uint32_t padding_per_row_; - uint32_t padding_column_above_; - uint32_t max_id_; - uint32_t num_traversable_; +protected: + void + setup_stream_(std::istream& in); + void + setup_ser_(io::bittable_serialize& parser); + + warthog::util::gm_header header_ = {}; + std::unique_ptr db_; + std::filesystem::path filename_; + + uint32_t dbwidth_ = 0; + uint32_t dbwidth64_ = 0; + uint32_t dbheight_ = 0; + uint32_t db_size_ = 0; + uint32_t padding_per_row_ = 0; + uint32_t padding_column_above_ = 0; + uint32_t max_id_ = 0; + uint32_t num_traversable_ = 0; void init_db(); diff --git a/include/warthog/domain/labelled_gridmap.h b/include/warthog/domain/labelled_gridmap.h index c689997..c5b2b14 100644 --- a/include/warthog/domain/labelled_gridmap.h +++ b/include/warthog/domain/labelled_gridmap.h @@ -12,18 +12,21 @@ // - a line of terminator characters are added before the first row. // - a line of terminator characters are added after the last row. // -// @author: dharabor +// @author: dharabor & Ryan Hechenberger // @created: 2018-11-08 // +#include "grid.h" #include +#include #include #include #include #include +#include #include -#include +#include namespace warthog::domain { @@ -32,35 +35,71 @@ template class labelled_gridmap { public: - labelled_gridmap(unsigned int h, unsigned int w) : header_(h, w, "octile") + labelled_gridmap() = default; + labelled_gridmap(uint32_t h, uint32_t w) : header_(h, w, "octile") { this->init_db(); } - - labelled_gridmap(const char* filename) + labelled_gridmap(std::istream& input) { setup_stream_(input); } + labelled_gridmap(io::bittable_serialize& parser) { setup_ser_(parser); } + labelled_gridmap(std::filesystem::path&& filename) { - util::gm_parser parser(filename); - header_ = parser.get_header(); - strcpy(filename_, filename); - init_db(); - - for(unsigned int i = 0; i < parser.get_num_tiles(); i++) - { - CELL cell = parser.get_tile_at(i); - ; - set_label(uint32_t{to_padded_id(pack_id{i})}, cell); - assert(get_label(uint32_t{to_padded_id(pack_id{i})}) == cell); - } + filename_ = std::move(filename); + std::ifstream in(filename_); + setup_stream_(in); } + labelled_gridmap(const std::filesystem::path& filename) + : labelled_gridmap(std::filesystem::path(filename)) + { } + labelled_gridmap(const char* filename) + : labelled_gridmap(std::filesystem::path(filename)) + { } labelled_gridmap(const labelled_gridmap&) = delete; + ~labelled_gridmap() = default; + + /// The number of padded rows before and after + static constexpr uint32_t PADDED_ROWS = 3; + labelled_gridmap& operator=(const labelled_gridmap&) = delete; - ~labelled_gridmap() { delete[] db_; } + void + setup(uint32_t h, uint32_t w) + { + header_.height_ = h; + header_.width_ = w; + this->init_db(); + } + void + load(std::istream& input) + { + setup_stream_(input); + } + void + load(io::bittable_serialize& parser) + { + setup_ser_(parser); + } + void + load(std::filesystem::path&& filename) + { + filename_ = std::move(filename); + std::ifstream in(filename_); + setup_stream_(in); + } + void + load(const std::filesystem::path& filename) + { + load(std::filesystem::path(filename)); + } + void + load(const char* filename) + { + load(std::filesystem::path(filename)); + } - // here we convert from the coordinate space of - // the original grid to the coordinate space of db_. + /// @brief convert unpadded id to padded id pad_id to_padded_id(pack_id node_id) const noexcept { @@ -68,54 +107,87 @@ class labelled_gridmap return pad_id{ uint32_t{node_id} + // padded rows before the actual map data starts - padded_rows_before_first_row_ * padded_width_ + + PADDED_ROWS * width() + // padding from each row of data before this one - (node_id.id / header_.width_) * padding_per_row_}; + (uint32_t{node_id} / header_.width_) * padding_per_row_}; } - // here we convert from the coordinate space of - // the original grid to the coordinate space of db_. + /// @brief convert unpadded (x,y) to a padded id pad_id to_padded_id_from_unpadded(uint32_t x, uint32_t y) const noexcept { - return pad_id{(y + padded_rows_before_first_row_) * padded_width_ + x}; + return pad_id{(y + PADDED_ROWS) * width() + x}; } + /// @brief convert padded (x,y) to a padded id pad_id to_padded_id_from_padded(uint32_t x, uint32_t y) const noexcept { - return pad_id{y * padded_width_ + x}; + return pad_id{y * width() + x}; } + /// @brief convert unpadded id to unpadded (x,y) void to_unpadded_xy(pack_id grid_id, uint32_t& x, uint32_t& y) const noexcept { y = uint32_t{grid_id} / header_.width_; x = uint32_t{grid_id} % header_.width_; + assert(x < header_.width_ && y < header_.height_); } + /// @brief convert padded id to unpadded (x,y) void to_unpadded_xy(pad_id grid_id, uint32_t& x, uint32_t& y) const noexcept { to_padded_xy(grid_id, x, y); - y -= padded_rows_before_first_row_; + y -= PADDED_ROWS; + assert(x < header_.width_ && y < header_.height_); + } + + /// @brief convert padded (x,y) to unpadded (x,y) + void + to_unpadded_xy_from_padded( + uint32_t padded_x, uint32_t padded_y, uint32_t& x, + uint32_t& y) const noexcept + { + y = padded_y - PADDED_ROWS; + x = padded_x; assert(x < header_.width_ && y < header_.height_); } + /// @brief convert padded id to padded (x,y) void to_padded_xy(pad_id grid_id, uint32_t& x, uint32_t& y) const noexcept { - y = uint32_t{grid_id} / padded_width_; - x = uint32_t{grid_id} % padded_width_; - assert(x < padded_width_ && y < padded_height_); + y = uint32_t{grid_id} / width(); + x = uint32_t{grid_id} % width(); + assert(x < width() && y < height()); + } + + /// @brief convert unpadded (x,y) to padded (x,y) + void + to_padded_xy_from_unpadded( + uint32_t unpadded_x, uint32_t unpadded_y, uint32_t& x, + uint32_t& y) const noexcept + { + y = unpadded_y + PADDED_ROWS; + x = unpadded_x; + assert(x < width() && y < height()); } + /// @brief convert padded id to unpadded id pack_id to_unpadded_id(pad_id grid_id) const noexcept { - uint32_t x, y; - to_unpadded_xy(grid_id, x, y); - return pack_id{y * header_.width_ + x}; + assert(width() != 0); + return pack_id{ + uint32_t{grid_id} - + // padding from each row of data + (uint32_t{grid_id} / width()) * padding_per_row_ - + // padded rows before the actual map data starts, use header_width + // as the padded width is already removed + PADDED_ROWS * header_.width_}; } + /// @brief convert unpadded (x,y) to unpadded id pack_id to_unpadded_id_from_unpadded(uint32_t x, uint32_t y) const noexcept { @@ -165,7 +237,7 @@ class labelled_gridmap return this->header_.width_; } - const char* + const std::filesystem::path filename() const noexcept { return this->filename_; @@ -177,44 +249,88 @@ class labelled_gridmap return sizeof(*this) + sizeof(CELL) * db_size_; } -private: - char filename_[256]; - util::gm_header header_; - CELL* db_; - - uint32_t db_size_; - uint32_t padding_per_row_; - uint32_t padded_rows_before_first_row_; - uint32_t padded_rows_after_last_row_; - uint32_t padded_width_; - uint32_t padded_height_; - +protected: void - init_db() - { - // when storing the grid we pad the edges of the map. - // this eliminates the need for bounds checking when - // fetching the neighbours of a node. - this->padded_rows_before_first_row_ = 3; - this->padded_rows_after_last_row_ = 3; - this->padding_per_row_ = 1; + setup_stream_(std::istream& in); + void + setup_ser_(io::bittable_serialize& parser); - this->padded_width_ = this->header_.width_ + this->padding_per_row_; - this->padded_height_ = this->header_.height_ - + this->padded_rows_after_last_row_ - + this->padded_rows_before_first_row_; + util::gm_header header_ = {}; + std::unique_ptr db_; + std::filesystem::path filename_; - this->db_size_ = this->padded_height_ * padded_width_; + uint32_t db_size_ = 0; + uint32_t padding_per_row_ = 0; + uint32_t padded_rows_before_first_row_ = 0; + uint32_t padded_rows_after_last_row_ = 0; + uint32_t padded_width_ = 0; + uint32_t padded_height_ = 0; - // create a one dimensional dbword array to store the grid - this->db_ = new CELL[db_size_]; + void + init_db(); +}; - for(uint32_t i = 0; i < this->db_size_; i++) - { - this->db_[i] = 0; - } +template +inline void +labelled_gridmap::setup_stream_(std::istream& in) +{ + io::bittable_serialize parser; + parser.open_read(&in); + if(parser.read_header() != std::errc{}) + throw std::runtime_error("invalid grid format"); + setup_ser_(parser); +} + +template +inline void +labelled_gridmap::setup_ser_(io::bittable_serialize& parser) +{ + if(parser.read_grid_header() != std::errc{}) + throw std::runtime_error("invalid grid format"); + this->header_.type_ = "octile"; + this->header_.width_ = parser.get_dim().width; + this->header_.height_ = parser.get_dim().height; + + init_db(); + // read raw data to buffer + std::unique_ptr buffer_v + = std::make_unique(this->db_size_); + std::span buffer(buffer_v.get(), this->db_size_); + if(auto ec = parser.read_grid_raw(buffer); ec != std::errc{}) + throw std::runtime_error("invalid grid format"); + // copy buffet to db + std::copy_n(buffer.data(), buffer.size(), this->db_.get()); +} + +template +inline void +labelled_gridmap::init_db() +{ + // when storing the grid we pad the edges of the map with + // zeroes. this eliminates the need for bounds checking when + // fetching the neighbours of a node. + uint32_t store_width, store_height; + store_height = this->header_.height_ + 2 * PADDED_ROWS; + + // calculate # of extra/redundant padding bits required, + // per row, to align map width with dbword size + store_width = this->header_.width_ + 1; + if((store_width % 8) != 0) + { + store_width = (this->header_.width_ / 8 + 1) * 8; } -}; + this->padded_width_ = store_width; + this->padded_height_ = store_height; + this->padded_rows_before_first_row_ = PADDED_ROWS; + this->padded_rows_after_last_row_ = PADDED_ROWS; + this->padding_per_row_ = store_width - this->header_.width_; + + this->db_size_ = store_width * store_height; + + // create a one dimensional dbword array to store the grid + this->db_ = std::make_unique(db_size_); + std::memset(this->db_.get(), 0, this->db_size_); +} // vertex-labelled gridmap using vl_gridmap = labelled_gridmap; diff --git a/include/warthog/io/fwd.h b/include/warthog/io/fwd.h new file mode 100644 index 0000000..21840d1 --- /dev/null +++ b/include/warthog/io/fwd.h @@ -0,0 +1,42 @@ +#ifndef WARTHOG_IO_FWD_H +#define WARTHOG_IO_FWD_H + +/// @file fwd.h +/// +/// Forward class definitions and store global enums. +/// +/// @author: Ryan Hechenberger +/// @created: 2026-05-01 + +#include + +namespace warthog::io +{ + +// serializers +class serialize_base; +class scenario_serialize; + +// observers +class stream_observer; +class posthoc_trace; + +enum class scenario_version : uint8_t +{ + UNKNOWN, + VERSION_1, + VERSION_2, +}; +enum class cost_type : uint8_t +{ + G_8C_NCC, + G_8C_CC, + G_4C, + AA_NCC, + AA_CC, + OTHER, +}; + +} // namespace warthog::io + +#endif // WARTHOG_IO_FWD_H diff --git a/include/warthog/io/grid.h b/include/warthog/io/grid.h index 401688a..3a65ae8 100644 --- a/include/warthog/io/grid.h +++ b/include/warthog/io/grid.h @@ -1,75 +1,92 @@ #ifndef WARTHOG_IO_GRID_H #define WARTHOG_IO_GRID_H -#include -#include +/// @file io/grid.h +/// +/// Read utility for gridmap. +/// +/// Supported MovingAI map format. Read format spec: +/// https://movingai.com/benchmarks/formats.html +/// +/// @author: Ryan Hechenberger +/// @created: 2025-06-01 + +#include "serialize_base.h" + #include #include +#include +#include +#include + namespace warthog::io { +/// @brief the type of bittable to (de)serialize enum class bittable_type : uint8_t { - AUTO, - OCTILE, - PATCH, - OTHER, - NONE, + OCTILE, ///< original MovingAI format + PATCH, ///< patch format, grouping multiple octiles + OTHER, ///< unknown format + NONE, ///< no format specified }; -enum class bittable_cell : uint8_t +/// @brief the cell character, as specified by MovingAI +enum class gridmap_cell : char { - BLOCKER, - TRAVERSABLE, - UNKNOWN + TERRAIN = '.', + TERRAIN_2 = 'G', + OUT_OF_BOUNDS = '@', + OUT_OF_BOUNDS_2 = 'O', + TREES = 'T', + SWAMP = 'S', + WATER = 'W', }; -class bittable_serialize +/// @brief Standard traversable terrain type from gridmap_cell +/// @param c +/// @return ".G" return true, false otherwise +constexpr inline bool +gridmap_cell_traversable(gridmap_cell c) noexcept +{ + switch(c) + { + case gridmap_cell::TERRAIN: + case gridmap_cell::TERRAIN_2: + return true; + default: + return false; + } +} +/// @return char c is traversable, as gridmap_cell_traversable((gridmap_cell)c) +constexpr inline bool +gridmap_cell_traversable(char c) noexcept +{ + return gridmap_cell_traversable(static_cast(c)); +} + +/// @brief max grid size +inline constexpr uint32_t GRID_MAX_SIZE = 15'000; + +/// @brief limit on max number of patches +inline constexpr uint32_t PATCH_COUNT_LIMIT = 10'000'000; + +/// @brief the bittable serialize class, flexable read/write of +/// bittable/gridmap or similiar datatypes, see serialize_base for +/// how files are read. +class bittable_serialize : public serialize_base { public: + bittable_serialize(); + /// @return the grid dimension, either as last read grid from file or set + /// by user for writing memory::bittable_dimension get_dim() const noexcept { return m_dim; } - bittable_type - get_type() const noexcept - { - return m_type; - } - - static constexpr bool - is_traversable(char c) noexcept - { - switch(c) - { - case '.': - case 'G': - return true; - default: - return false; - } - } - static constexpr bittable_cell - cell_type(char c) noexcept - { - switch(c) - { - case '.': - case 'G': - return bittable_cell::TRAVERSABLE; - case '@': - case 'O': - case 'S': - case 'T': - case 'W': - return bittable_cell::BLOCKER; - default: - return bittable_cell::UNKNOWN; - } - } - + /// @brief sets the grid dimension, throws if out of range void set_dim(uint32_t width, uint32_t height) { @@ -82,67 +99,165 @@ class bittable_serialize m_dim.width = width; m_dim.height = height; } - void + + /// @return the type/version of the file, default OCTILE + bittable_type + get_type() const noexcept + { + return m_type; + } + /// @brief sets the type/version to write to the file header, supported is + /// octile/patch. + /// @return std::errc{} for success, otherwise failure + std::errc set_type(bittable_type type) { - if(static_cast(type) - > static_cast(bittable_type::NONE)) + if(type != bittable_type::OCTILE && type != bittable_type::PATCH) { - throw std::out_of_range("type"); + return std::errc::argument_out_of_domain; } m_type = type; + return std::errc{}; } - bool - read_header(std::istream& in); + /// @brief get the number of patches in file + uint32_t + get_patch_amount() const noexcept + { + return m_patch_amount; + } + /// @brief set the number of patches (for writing) + /// @return std::errc{} for success, otherwise failure + std::errc + set_patch_amount(uint32_t count) + { + if(count > PATCH_COUNT_LIMIT) + { + return std::errc::argument_out_of_domain; + } + m_patch_amount = count; + return std::errc{}; + } + /// @brief gets the number of patches that have been read/write + uint32_t + get_patch_count() const noexcept + { + return m_patch_count; + } + /// @brief gets the id of last patch (usually get_patch_count()) + uint32_t + get_patch_id() const noexcept + { + return m_patch_id; + } + /// @brief gets the number of patches that have been read + /// @return std::errc{} for success, otherwise failure + std::errc + set_patch_id(uint32_t id) noexcept + { + m_patch_id = id; + return std::errc{}; + } + + /// @brief reads the map/patch file header, getting the type + /// @param in alternative file stream to read from + /// @return value init on success, error code on failure + /// + /// Reads the header line, `type octile` for bittable_type::OCTILE or + /// `type patch` for bittable_type::PATCH, retrievable by get_type(). + /// For PATCH type, also reads following line for number of patches in + /// file. + std::errc + read_header(std::istream* in = nullptr); + /// @brief Reads the grids' header, getting width/height up to the map + /// data. + /// @param in alternative filestream to read from + /// @return value init on success, error code on failure + /// @pre get_type() matches the format of file. + std::errc + read_grid_header(std::istream* in = nullptr); + + /// @brief Reads the grids data and stores it into a bittable, expects size + /// from get_dim() + /// @param table bittable derived type to store, must be init + /// @param offset_x offset of top-left in table to copy grid to + /// @param offset_y offset of top-left in table to copy grid to + /// @param in alternative filestream to read from + /// @return value init on success, error code on failure + /// @pre table must be init and large enough to store whole grid (including + /// from offset) template - bool - read_map( - std::istream& in, BitTable& table, uint32_t offset_x = 0, - uint32_t offset_y = 0); + std::errc + read_grid_data( + BitTable& table, uint32_t offset_x = 0, uint32_t offset_y = 0, + std::istream* in = nullptr); + + /// @brief Reads the raw rows (char) from map into a 1D array, expects size + /// from get_dim() + /// @param buffer the buffer + /// @param in alternative filestream to read from + /// @return value init on success, error code on failure + /// @pre buffer must be large enough to store width x height characters + /// from get_dim() + /// + /// Reads row by row from the top left, writing into buffer. + /// Data is tightly packed, with no delimited between rows of size width. + /// Characters are as defined by the MovingAI spec, use + /// gridmap_cell_traversable(c) to determine traversability if applicable. + std::errc + read_grid_raw(std::span buffer, std::istream* in = nullptr); protected: memory::bittable_dimension m_dim = {}; - bittable_type m_type = bittable_type::AUTO; + bittable_type m_type = bittable_type::NONE; + uint32_t m_patch_amount = 0; uint32_t m_patch_count = 0; + uint32_t m_patch_id = 0; }; template -bool -bittable_serialize::read_map( - std::istream& in, BitTable& table, uint32_t offset_x, uint32_t offset_y) +std::errc +bittable_serialize::read_grid_data( + BitTable& table, uint32_t offset_x, uint32_t offset_y, std::istream* in) { + // check table const memory::bittable_dimension dim = table.dim(); const memory::bittable_dimension read_dim = m_dim; - table.fill(0); // set whole table to 0 (blocker) // detect for overflow if(offset_x >= dim.width || read_dim.width + offset_x > dim.width) - return false; + return std::errc::argument_out_of_domain; if(offset_y >= dim.height || read_dim.height + offset_y > dim.height) - return false; - assert(read_dim.width <= GRID_DIMENSION_MAX); - if(read_dim.width > GRID_DIMENSION_MAX) - return false; // shound never happen + return std::errc::argument_out_of_domain; uint32_t bit_id = static_cast(table.xy_to_id(offset_x, offset_y)); const uint32_t bit_row_offset = dim.width - read_dim.width; - char buffer[(GRID_DIMENSION_MAX + 16) & ~7ull]; - static_assert(sizeof(buffer) / sizeof(char) >= GRID_DIMENSION_MAX); + + std::errc err; + std::tie(in, err) = get_istream(in); + if(err != std::errc{}) { return err; } + std::string_view line; + std::string_view token; + for(uint32_t y = 0; y < read_dim.height; ++y, bit_id += bit_row_offset) { - in >> std::ws; - in.read(buffer, read_dim.width); + // read row + std::tie(line, err) = readline(in); + if(err != std::errc{}) { return err; } + parser par(line); + if(!par.next(token).eof()) { return par.error(); } + if(token.size() != read_dim.width) + return std::errc::argument_out_of_domain; + // copy row to table for(uint32_t x = 0; x < read_dim.width; ++x, ++bit_id) { - auto cell = cell_type(buffer[x]); - if(cell == bittable_cell::UNKNOWN) - return false; - else if(cell == bittable_cell::TRAVERSABLE) - table.bit_or(static_cast(bit_id), 1); + table.set( + static_cast(bit_id), + gridmap_cell_traversable(token[x]) ? 1 : 0); } } - return true; + + return std::errc{}; } } // namespace warthog::memory diff --git a/include/warthog/io/grid_trace.h b/include/warthog/io/grid_trace.h index 6a76ff0..28ce37d 100644 --- a/include/warthog/io/grid_trace.h +++ b/include/warthog/io/grid_trace.h @@ -1,15 +1,15 @@ #ifndef WARTHOG_IO_GRID_TRACE_H #define WARTHOG_IO_GRID_TRACE_H -// io/grid_trace.h -// -// Basic posthoc_trace for use with gridmap. -// -// @author: Ryan Hechenberger -// @created: 2025-08-07 -// +/// @file io/grid_trace.h +/// +/// Basic posthoc_trace for use with gridmap. +/// +/// @author: Ryan Hechenberger +/// @created: 2025-08-07 #include "posthoc_trace.h" + #include #include #include diff --git a/include/warthog/io/log.h b/include/warthog/io/log.h index 8c61726..1d36142 100644 --- a/include/warthog/io/log.h +++ b/include/warthog/io/log.h @@ -1,49 +1,48 @@ #ifndef WARTHOG_IO_LOG_H #define WARTHOG_IO_LOG_H -// io/log.h -// -// Logging utility framework, where a user can provide at a high-level -// data structure with function pointers to log messages as a single string -// with a specific log level. -// This will call a user-defined function (if able) that will output this -// message as the user desires. -// Default functions (output to stderr or file) are defined here. -// -// The log_sink is a non-owning copyable struct that points to the data and -// logging function calls. All logging is performed through log_sink. All -// classes here are thread safe, follow comments for outlying cases. -// -// Special classes inherit log_sink to provide default functionality. -// log_sink_std should be used to write to std::cout and std::cerr. -// log_sink_stream pipe to a stream or open a file stream. -// -// The logs are made to be logged to a certain log_level. -// The WARTHOG_TRACE and others in this header provide interfaces to log to a -// special logger<> class. This logger<> class knows at compile time the -// minimum level to log, and with the use of a macro will be compiled out if -// that level was not set at compile time when used with the logging macros. -// -// The global logger can be acquired with the glog() (logger<>) or glogs() -// (log_sink), and set with set_glog(log_sink). The log level of the global -// logger is set though the WARTHOG_LOG definition, with a default of 1(debug) -// for debug builds, and 2(information) for release (NDEBUG defined). Macros -// like WARTHOG_GTRACE will automatically write to the global logger. -// -// The _IF will only log if a runtime if true. -// The _FMT uses the std::format from C++20 to format the output messages. -// The logging utilities uses dynamic memory std::strings to produce the final -// log message strings, thus is better to be disabled for release builds -// through the log level. -// -// Log messages produce a messaged as "[TIME LEVEL] msg". -// The time is formatted in ISO with space (yyyy-mm-dd hh:mm:ss), but can be -// overridden with define cstring WARTHOG_LOG_TIME. Clock is in local system -// time, but can be set to UTC by definiong WARTHOG_LOG_CLOCK_UTC. -// -// @author: Ryan Hechenberger -// @created: 2025-09-09 -// +/// @file log.h +/// +/// Logging utility framework, where a user can provide at a high-level +/// data structure with function pointers to log messages as a single string +/// with a specific log level. +/// This will call a user-defined function (if able) that will output this +/// message as the user desires. +/// Default functions (output to stderr or file) are defined here. +/// +/// The log_sink is a non-owning copyable struct that points to the data and +/// logging function calls. All logging is performed through log_sink. All +/// classes here are thread safe, follow comments for outlying cases. +/// +/// Special classes inherit log_sink to provide default functionality. +/// log_sink_std should be used to write to std::cout and std::cerr. +/// log_sink_stream pipe to a stream or open a file stream. +/// +/// The logs are made to be logged to a certain log_level. +/// The WARTHOG_TRACE and others in this header provide interfaces to log to a +/// special logger<> class. This logger<> class knows at compile time the +/// minimum level to log, and with the use of a macro will be compiled out if +/// that level was not set at compile time when used with the logging macros. +/// +/// The global logger can be acquired with the glog() (logger<>) or glogs() +/// (log_sink), and set with set_glog(log_sink). The log level of the global +/// logger is set though the WARTHOG_LOG definition, with a default of 1(debug) +/// for debug builds, and 2(information) for release (NDEBUG defined). Macros +/// like WARTHOG_GTRACE will automatically write to the global logger. +/// +/// The _IF will only log if a runtime if true. +/// The _FMT uses the std::format from C++20 to format the output messages. +/// The logging utilities uses dynamic memory std::strings to produce the final +/// log message strings, thus is better to be disabled for release builds +/// through the log level. +/// +/// Log messages produce a messaged as "[TIME LEVEL] msg". +/// The time is formatted in ISO with space (yyyy-mm-dd hh:mm:ss), but can be +/// overridden with define cstring WARTHOG_LOG_TIME. Clock is in local system +/// time, but can be set to UTC by defining WARTHOG_LOG_CLOCK_UTC. +/// +/// @author: Ryan Hechenberger +/// @created: 2025-09-09 #include #include @@ -56,6 +55,16 @@ #include #include +// define utility for help with log messages +#define WARTHOG_STRING_(x) #x +#define WARTHOG_STRING(x) WARTHOG_STRING_(x) +#define WARTHOG_STRING2(x) WARTHOG_STRING(x) +#define WARTHOG_STRING3(x) WARTHOG_STRING(x) +#define WARTHOG_STRING4(x) WARTHOG_STRING(x) + +#define WARTHOG_LINE WARTHOG_STRING2(__LINE__) +#define WARTHOG_FILENAME_LINE __FILE__ "@" WARTHOG_LINE + // default log levels, also for global logger #ifdef WARTHOG_LOG #define WARTHOG_DEFAULT_LOG_LEVEL WARTHOG_LOG diff --git a/include/warthog/io/observer.h b/include/warthog/io/observer.h index d617bd1..d4aa096 100644 --- a/include/warthog/io/observer.h +++ b/include/warthog/io/observer.h @@ -1,28 +1,27 @@ #ifndef WARTHOG_IO_OBSERVER_H #define WARTHOG_IO_OBSERVER_H -// io/observer.h -// -// Defines use of an observer pattern, in which observer object is registered -// to an observable, and the observable will trigger events to all relevant -// observers. -// The observer is passed to an observable as a list of tuples, -// and when triggering an event will notify all observers by function call of -// the event name that is callable to the observer. -// Observer are either stored as value in the tuple or pointer to an observer. -// -// These function names must be registered before use, common ones registered -// here. -// -// To register a new function name, use WARTHOG_OBSERVER_DEFINE([function]). -// Invoke event with observer_[function](listeners, args...) where listeners -// are tuple of observers. This will run through each element in tuple (i) and -// call i.[function](args...) if able. If i.event([function],args...) is a -// valid callable, calls this function first, also tries i.event([function]). -// -// @author: Ryan Hechenberger -// @created: 2025-08-06 -// +/// @file io/observer.h +/// +/// Defines use of an observer pattern, in which observer object is registered +/// to an observable, and the observable will trigger events to all relevant +/// observers. +/// The observer is` passed to an observable as a list of tuples, +/// and when triggering an event will notify all observers by function call of +/// the event name that is callable to the observer. +/// Observer are either stored as value in the tuple or pointer to an observer. +/// +/// These function names must be registered before use, common ones registered +/// here. +/// +/// To register a new function name, use WARTHOG_OBSERVER_DEFINE([function]). +/// Invoke event with observer_[function](listeners, args...) where listeners +/// are tuple of observers. This will run through each element in tuple (i) and +/// call i.[function](args...) if able. If i.event([function],args...) is a +/// valid callable, calls this function first, also tries i.event([function]). +/// +/// @author: Ryan Hechenberger +/// @created: 2025-08-06 #include diff --git a/include/warthog/io/posthoc_trace.h b/include/warthog/io/posthoc_trace.h index fcdf35a..926329f 100644 --- a/include/warthog/io/posthoc_trace.h +++ b/include/warthog/io/posthoc_trace.h @@ -1,14 +1,13 @@ #ifndef WARTHOG_IO_POSTHOC_TRACE_H #define WARTHOG_IO_POSTHOC_TRACE_H -// io/posthoc_trace.h -// -// stream_observer that outputs a trace for use with posthoc visuliser. -// See https://posthoc-app.pathfinding.ai/ -// -// @author: Ryan Hechenberger -// @created: 2025-08-07 -// +/// @file posthoc_trace.h +/// +/// stream_observer that outputs a trace for use with posthoc visuliser. +/// See https:///posthoc-app.pathfinding.ai/ +/// +/// @author: Ryan Hechenberger +/// @created: 2025-08-07 #include "stream_observer.h" diff --git a/include/warthog/io/scenario.h b/include/warthog/io/scenario.h new file mode 100644 index 0000000..12e6de1 --- /dev/null +++ b/include/warthog/io/scenario.h @@ -0,0 +1,371 @@ +#ifndef WARTHOG_IO_SCENARIO_H +#define WARTHOG_IO_SCENARIO_H + +/// @file scenario.h +/// +/// Read/write utilities for scenario files. +/// +/// Supported formats for read: +/// - GPPC 1.0 format (as at 2012 Grid-based Path Planning Competition) +/// (fields: bucket,map,mapwidth,mapheight,sx,sy,gx,gy,distance) +/// - DIMACS format (as at the 9th DIMACS Implementation Challenge) +/// (fields: q [source-id] [target-id]) +/// - Dynamic format (tbd link) +/// +/// Supported formats for generate/write: +/// - GPPC 1.0 format (as at 2012 Grid-based Path Planning Competition) +/// - Dynamic format (tbd link) +/// +/// @author: dharabor & Ryan Hechenberger +/// @created: 2025-12-04 +/// + +#include "serialize_base.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace warthog::io +{ + +/// @brief scenario instance struct, reusable with fields set by +/// scenario_serialize +struct scenario_instance +{ + int64_t bucket; + std::string_view map; + int32_t width; + int32_t height; + double start_x; + double start_y; + double goal_x; + double goal_y; + std::span cost; + void* extra_data; ///< holds extra data that a user may require (not used + ///< by default) + + void + reset() + { + (*this) = {}; + } +}; + +struct scenario_patch +{ + int64_t bucket; + uint32_t patch_id; + uint16_t loc_x; + uint16_t loc_y; +}; + +class scenario_serialize : public serialize_base +{ +public: + enum class serialize_state : uint8_t + { + INIT, + VERSION, + COMMAND, + END, + ERROR, + }; + enum command_res : uint8_t + { + INVALID, + VALID, + FINAL, + CMD_INST, + CMD_PATCH, + CMD_UNKNOWN, + }; + scenario_serialize(); + ~scenario_serialize() override; + + static constexpr std::string_view + get_cost_str(cost_type a) noexcept; + static constexpr cost_type + get_cost_type(std::string_view a) noexcept; + + /// @brief Resets class, including memory. Must use between seperate + /// read/writes, needed for memory managment. + void + reset(); + + /// @return the current state of scenario read/write + serialize_state + state() const noexcept + { + return m_state; + } + + void + set_scenario_filename(std::filesystem::path&& filename) + { + set_filename(std::move(filename)); + } + const std::filesystem::path& + get_scenario_filename() const noexcept + { + return get_filename(); + } + + void + set_map_filename(std::filesystem::path&& filename) + { + m_map_filename = std::move(filename); + } + const std::filesystem::path& + get_map_filename() const noexcept + { + return m_map_filename; + } + + void + set_relative_map_filename(const std::filesystem::path& filename); + + void + set_version(scenario_version version) noexcept + { + m_version = version; + } + scenario_version + get_version() const noexcept + { + return m_version; + } + + /// @brief the types of dist used, only relivent for version2 + std::span + get_cost_strings() const noexcept + { + return m_cost_strings; + } + std::span + get_cost_type() const noexcept + { + return m_cost_type; + } + std::span + get_cost_value() const noexcept + { + return m_cost_value; + } + int + find_cost_index(cost_type c) const noexcept + { + auto it = std::find(m_cost_type.begin(), m_cost_type.end(), c); + return it != m_cost_type.end() + ? static_cast(it - m_cost_type.begin()) + : -1; + } + int + find_cost_index(std::string_view c) const noexcept + { + auto it = std::find(m_cost_strings.begin(), m_cost_strings.end(), c); + return it != m_cost_strings.end() + ? static_cast(it - m_cost_strings.begin()) + : -1; + } + + uint32_t + get_map_width() const noexcept + { + return m_map_width; + } + int32_t + get_map_height() const noexcept + { + return m_map_height; + } + + void + set_force_int(bool v) noexcept + { + m_force_int = v; + } + bool + get_force_int() noexcept + { + return m_force_int; + } + + void + close() override; + + virtual int + last_command_type() const; + + /// @brief reads in file version information, and sets version (accessible + /// via get_version()) + /// @param in optional stream to use, otherwise uses internal-set stream + /// @return success std::errc{}, else failure + /// @pre state() == serialize_state::INIT (returns errc otherwise) + virtual std::errc + read_version(std::istream* in = nullptr); + /// @brief read header (without version) information. + /// @param in optional stream to use, otherwise uses internal-set stream + /// @return success std::errc{}, else failure + /// @pre state() == serialize_state::VERSION (returns errc otherwise) + /// + /// With VERSION1: peeks first instance to gain map name + /// With VERSION2: gets map width/height, available costs and patch + /// filename + virtual std::errc + read_header(std::istream* in = nullptr); + + /// @brief read header as VERSION_1, does not consider the state or + /// version. use read_header for checks instead. + std::errc + read_header_v1(std::istream* in = nullptr); + /// @brief read header as VERSION_2, does not consider the state or + /// version. use read_header for checks instead. + std::errc + read_header_v2(std::istream* in = nullptr); + + /// @brief gets the next command type + /// @param in optional stream to use, otherwise uses internal-set stream + /// @return a pair with a value from command_res and std::errc for success + /// (else error) + /// @pre state() == serialize_state::COMMAND (returns error otherwise) + /// + /// With VERSION_1: + /// Returns CMD_INST or FINAL if no more commands are present + /// With VERSION_2: + /// Returns command based on last_command_type(), or FINAL if not + /// present. Default expects CMD_INST or CMD_PATCH. + std::pair + next_command_type(std::istream* in = nullptr); + /// @brief skips the next count number of commands + /// @param count number of commands to skip + /// @param in optional stream to use, otherwise uses internal-set stream + /// @return success std::errc{}, else failure + /// @pre state() == serialize_state::COMMAND (returns errc otherwise) + std::errc + skip_commands(int count = 1, std::istream* in = nullptr); + + /// @brief reads an instance line and stores results in scenario_instance + /// @param inst where to store the instance data read in + /// @param in optional stream to use, otherwise uses internal-set stream + /// @return a pair with a value from command_res and std::errc for success + /// (else error) + /// @pre state() == serialize_state::COMMAND (returns error otherwise) + /// + /// if next_command_type().first == CMD_INST, then reads the instance, + /// otherwise returns CMD_? dependent on the type of command, or FINAL. + /// With get_version() == VERSION_1: + /// Returns VALID for success, FINAL for no more commands, and INVALID + /// for invalid instance. + /// With get_version() == VERSION_2: + /// Returns VALID for success, FINAL for no more commands, and INVALID + /// for invalid instance, or last_command_type() (only CMD_PATCH for + /// standard v2 scenario) of type of command. + /// + /// INVALID return without error code means success in reading command, but + /// command has invalid parameters. Main checks are non-finite floats + /// (start/goal/cost), mismatch width/height, or out of bounds start/goal. + /// If get_force_int() == true, also checks start/goal are integers within + /// the grid, otherwise allows float also within the grid (allows x == + /// width() or y == height()). + virtual std::pair + read_instance_line(scenario_instance& inst, std::istream* in = nullptr); + + /// @brief as VERSION_1 with read_instance_line, does not check + /// pre-conditions + std::pair + read_instance_line_v1(scenario_instance& inst, std::istream* in = nullptr); + /// @brief as VERSION_2 with read_instance_line, does not check + /// pre-conditions + std::pair + read_instance_line_v2(scenario_instance& inst, std::istream* in = nullptr); + + /// @brief reads a patch line and stores results in scenario_patch + /// @param patch where to store the patch data read in (not grid) + /// @param in optional stream to use, otherwise uses internal-set stream + /// @return a pair with a value from command_res and std::errc for success + /// (else error) + /// @pre state() == serialize_state::COMMAND (returns error otherwise) + /// + /// if next_command_type().first == CMD_PATCH, then reads the instance, + /// otherwise returns CMD_? dependent on the type of command, or FINAL. + /// With get_version() == VERSION_2: + /// Returns VALID for success, FINAL for no more commands, INVALID if + /// location is out of grid bounds, or last_command_type() (only CMD_INST + /// for standard v2 scenario) of type of command. + virtual std::pair + read_patch_line(scenario_patch& patch, std::istream* in = nullptr); + + /// @brief as VERSION_2 with read_patch_line, does not check pre-conditions + std::pair + read_patch_line_v2(scenario_patch& patch, std::istream* in = nullptr); + +protected: + /// @return a owned version of str + std::string_view + copy_string(std::string_view str); + +protected: + serialize_state m_state = serialize_state::INIT; + scenario_version m_version = scenario_version::UNKNOWN; + bool m_force_int = false; + std::filesystem::path m_map_filename; + uint32_t m_map_width = 0; + uint32_t m_map_height = 0; + int32_t m_inst_at = 0; + + // dynamic data + std::pmr::monotonic_buffer_resource m_dyn_res; + std::pmr::monotonic_buffer_resource m_string_res; + // distance types + std::pmr::vector m_cost_strings; ///< names read in + std::pmr::vector + m_cost_type; ///< cost_type of index (corrisponding dist_strings and + ///< dist_value) + std::pmr::vector m_cost_value; ///< distance value stored, will be + ///< placed in dynamic_scenario + + std::string m_command_type; ///< last cost type +}; + +constexpr std::string_view +scenario_serialize::get_cost_str(cost_type a) noexcept +{ + switch(a) + { + case cost_type::G_8C_NCC: + return "8c-ncc"; + case cost_type::G_8C_CC: + return "8c-cc"; + case cost_type::G_4C: + return "4c"; + case cost_type::AA_NCC: + return "aa-ncc"; + case cost_type::AA_CC: + return "aa-cc"; + default: + return std::string_view(); + } +} +constexpr cost_type +scenario_serialize::get_cost_type(std::string_view a) noexcept +{ + if(a == "8c-ncc") return cost_type::G_8C_NCC; + if(a == "8c-cc") return cost_type::G_8C_CC; + if(a == "4c") return cost_type::G_4C; + if(a == "aa-ncc") return cost_type::AA_NCC; + if(a == "aa-cc") return cost_type::AA_CC; + return cost_type::OTHER; +} + +} // namespace warthog::io + +#endif // WARTHOG_IO_SCENARIO_H diff --git a/include/warthog/io/serialize_base.h b/include/warthog/io/serialize_base.h new file mode 100644 index 0000000..89ffc1b --- /dev/null +++ b/include/warthog/io/serialize_base.h @@ -0,0 +1,209 @@ +#ifndef WARTHOG_IO_SERIALIZE_BASE_H +#define WARTHOG_IO_SERIALIZE_BASE_H + +/// @file serialize_base.h +/// +/// Read/write base class for serialize classes. +/// Adds a uniform low-level interface for reading from a file, one-line at a +/// time. Support for both file or just from a istream/ostream object. Tracking +/// of line number for user-level error reporting and debugging. +/// +/// Ideal for use with serialize_base::parser (util::string_parser), +/// which supports fast reading of tokens through string_view and error +/// checking. +/// +/// @author: Ryan Hechenberger +/// @created: 2026-04-10 + +#include "fwd.h" + +#include + +#include +#include +#include +#include +#include + +namespace warthog::io +{ + +/// @brief serialize base class to support line-by-line reading with strict +/// error checking, line-number tracking and other features. +/// +/// The base class for serialize, low-level with strict error checking. +/// The ideal use case is to ensure data is read correctly, and error on +/// ill-formed without raising exceptions. +/// +/// To support low-level, each read/write takes istream|ostream pointer to +/// override the class default-defined, use get_istream|get_ostream to handle +/// the selection manually. Most functions return std::errc of non-value init +/// to state that procedure has failed. +/// +/// Use readline() to raed next line (or next blank line with true), limits +/// length to max_line_length (1k def) and strips \r?\n from end. Use +/// unreadline() to put a line back to be read by next readline (can be +/// changed). +class serialize_base +{ +public: + using parser = util::string_parser; + + static constexpr size_t max_line_length = 1 + << 10; ///< 1kB default maximum line length + serialize_base(); + virtual ~serialize_base(); + + void + set_filename(std::filesystem::path&& filename) + { + m_filename = std::move(filename); + } + const std::filesystem::path& + get_filename() const noexcept + { + return m_filename; + } + + int32_t + get_line_num() const noexcept + { + return m_line_num; + } + std::string_view + get_last_line() const noexcept + { + return m_line; + } + + /// @brief opens scenario file get_scenario_filename() for reading + /// @param scenario use a user provided instead of get_scenario_filename() + /// @return error on operation + virtual std::errc + open_read(std::istream* scenario = nullptr); + /// @brief opens scenario file get_scenario_filename() for writing + /// @param scenario use a user provided instead of get_scenario_filename() + /// @return error on operation + virtual std::errc + open_write(std::ostream* scenario = nullptr); + + virtual void + close(); + + /// @return if scenario is open for reading + bool + can_read(std::istream* in = nullptr) + { + if(in == nullptr) in = m_stream_in; + return in != nullptr; + } + /// @return if scenario is open for writing + bool + can_write(std::ostream* out = nullptr) + { + if(out == nullptr) out = m_stream_out; + return out != nullptr; + } + + /// @return the maximum line length (null terminator included), 0 means + /// unallocated and will default to max_line_length on readline() call + uint32_t + get_max_line_length() noexcept + { + return m_max_line_length; + } + +protected: + /// @return the internal istream or provided, or error + std::pair + get_istream(std::istream* in = nullptr) noexcept + { + if(in == nullptr) in = m_stream_in; + if(in == nullptr || !in) return {nullptr, std::errc::io_error}; + return {in, {}}; + } + /// @return the internal istream or provided, or error + std::pair + get_ostream(std::ostream* out = nullptr) noexcept + { + if(out == nullptr) out = m_stream_out; + if(out == nullptr || !out->good()) + return {nullptr, std::errc::io_error}; + return {out, {}}; + } + + /// @return true if istream is at eof, false if not or in error + bool + istream_eof(std::istream* in = nullptr); + + /// @brief reads lines and return 1 line, checking for errors + /// @param in optional stream to use, otherwise uses internal-set stream + /// @param skip_blanks if true, the first line where is_line_blank(line) is + /// false is returned + /// @return the first valid line (can be blank), or error, the final empty + /// line is returned empty and eof is set + /// + /// Reads lines into a buffer (max size get_max_line_length()), removing + /// the line ends \r\n. If unreadline was called before, returns that line, + /// otherwise reads from istream. Change the max buffer size with + /// set_max_line_length, although it clears the buffer and current line. + /// + /// If line does not fit, return an error. + /// If an empty line is discovered, it will return the same result as the + /// eof condition, use istream_eof to distinguish. The file ending on an + /// empty line is handled the same way as the last line having no trailing + /// \n. + /// + /// get_line_num() will return the read number read, reading from an + /// unreadline does not change the line number. + std::pair + readline(std::istream* in, bool skip_blanks = false); + + /// @brief unreads a line, keeps only a single line + /// @param line the line to return and read later, does not have to match a + /// line read from readline. + void + unreadline(std::string_view line); + + /// @brief check if while line is either blank or empty, as per + /// std::isspace + bool + is_line_blank(std::string_view line); + + /// @brief sets the max line length, clears current line buffer and line + void + set_max_line_length(uint32_t len) + { + m_max_line_length = len; + m_line_data = nullptr; + m_line = std::string_view(); + } + +protected: + std::filesystem::path m_filename; + std::unique_ptr m_stream; + std::istream* m_stream_in = nullptr; + std::ostream* m_stream_out = nullptr; + int32_t m_line_num = 0; + uint32_t m_max_line_length = 0; + std::unique_ptr m_line_data; + std::string_view m_line; + std::string m_unget_line; +}; + +/// @brief a serialize_base used solely for reading line-by-line +class line_serialize final : public serialize_base +{ +public: + using serialize_base::serialize_base; + + using serialize_base::is_line_blank; + using serialize_base::istream_eof; + using serialize_base::readline; + using serialize_base::set_max_line_length; + using serialize_base::unreadline; +}; + +} // namespace warthog::io + +#endif // WARTHOG_IO_SERIALIZE_BASE_H diff --git a/include/warthog/io/stream_observer.h b/include/warthog/io/stream_observer.h index 33678e8..09373f9 100644 --- a/include/warthog/io/stream_observer.h +++ b/include/warthog/io/stream_observer.h @@ -1,22 +1,24 @@ #ifndef WARTHOG_IO_STEAM_OBSERVER_H #define WARTHOG_IO_STEAM_OBSERVER_H -// io/stream_observer.h -// -// The stream observer is a base class for observers that can open and own a -// filestream, or pass another filestream. -// This is designed as a many-to-one observers to stream. -// No inbuilt support for multi-threading, use locks in the observer function. -// -// Is designed to be used with observer tuples. -// Inherited class will call stream() to get the current stream for output. -// Using the observer methodology, event functions will be given that will -// write to output in certain ways. -// -// @author: Ryan Hechenberger -// @created: 2025-08-01 +/// @file stream_observer.h +/// +/// The stream observer is a base class for observers that can open and own a +/// filestream, or pass another filestream. +/// This is designed as a many-to-one observers to stream. +/// No inbuilt support for multi-threading, use locks in the observer function. +/// +/// Is designed to be used with observer tuples. +/// Inherited class will call stream() to get the current stream for output. +/// Using the observer methodology, event functions will be given that will +/// write to output in certain ways. +/// +/// @author: Ryan Hechenberger +/// @created: 2025-08-01 // +#include "fwd.h" + #include "log.h" #include #include diff --git a/include/warthog/manager/experiment.h b/include/warthog/manager/experiment.h new file mode 100644 index 0000000..adcd703 --- /dev/null +++ b/include/warthog/manager/experiment.h @@ -0,0 +1,19 @@ +#ifndef WARTHOG_MANAGER_EXPERIMENT_H +#define WARTHOG_MANAGER_EXPERIMENT_H + +/// @file experiment.h +/// +/// Alias for util::experiemnt. +/// +/// @author: Ryan Hechenberger +/// @created: 2026-04-10 +/// + +#include + +namespace warthog::manager +{ +using warthog::util::experiment; +} // namespace warthog::manager + +#endif // WARTHOG_MANAGER_SCENARIO_MANAGER_H diff --git a/include/warthog/manager/grid_patch_set.h b/include/warthog/manager/grid_patch_set.h new file mode 100644 index 0000000..b9eb999 --- /dev/null +++ b/include/warthog/manager/grid_patch_set.h @@ -0,0 +1,136 @@ +#ifndef WARTHOG_MANAGER_GRID_PATCH_SET_H +#define WARTHOG_MANAGER_GRID_PATCH_SET_H + +/// @file grid_patch_set.h +/// +/// Utility to store grid map/patches. +/// Can read with some flexibility and be used with scenario_runner to update a +/// gridmap. +/// +/// @author: Ryan Hechenberger +/// @created: 2026-04-10 +/// + +#include +#include +#include + +#include +#include +#include + +namespace warthog::manager +{ + +/// @brief a class for managing a set of patches +/// +/// Stores a list of patches in a vector. +/// Supports loading of patches from a file/stream with flags to control this. +/// User can also add their own patches. +class grid_patch_set +{ +public: + enum flags : uint32_t + { + DEFAULT = 0u, ///< default options + SKIP_HEADER = 1u << 0, ///< will not read header + FORCE_HEADER = 1u << 1, ///< must read header + IGNORE_INDEX = 1u << 2, ///< ignores patch read index + }; + using bittable = domain::gridmap::bittable; + static constexpr uint16_t npos = (uint16_t)-1u; + + grid_patch_set(std::pmr::memory_resource* upstream = nullptr) + : grid_res_( + upstream != nullptr ? upstream + : std::pmr::get_default_resource()) + { } + + /// @brief read istream as whole patch set + /// @param file open text istream + /// @param max_grids maximum number of grids to read + /// @return true if success, false otherwise + bool + load(std::istream& file, int max_grids = -1); + + /// @brief opens file and read as whole patch set + /// @param maps the filename to open + /// @param max_grids maximum number of grids to read + /// @return true if success, false otherwise + bool + load(const std::filesystem::path& maps, int max_grids = -1); + + /// @brief reads from a serialize, checking for errors. Can read both type + /// octile and patch files. + /// @param S the bittable_serialize, must be open + /// @param max_grids the max grids to read, or -1 for no limit + /// @param flags flags that control how to read, check enum flags + /// @return the number of grids read successfully, or <0 for negative index + /// of patch that failed to be read successfully + /// + /// This function will load from an established bittable_serialize, reading + /// all grids and appending to existing patches. If type octile, only one + /// grid is read as the first patch, otherwise up to max_grids (default + /// all) are read. + /// + /// By DEFAULT, it will read the header if not already read and append all + /// remaining grids to existing patches, but will error if the index in the + /// file does not match the index in this class. SKIP_HEADER requires that + /// S have already read the header else errors. FORCE_HEADER requires that + /// S have not read the header else errors. IGNORE_INDEX will not error if + /// index in patch file does not match. + int + deserialize( + io::bittable_serialize& S, int max_grids = -1, + uint32_t flags = DEFAULT); + + /// @brief copies a user-provided bittable, subregion from offset with + /// width/height + /// @param table the base table to push + /// @return true on success, false otherwise + /// @pre (offset_x == 0 && offset_y == 0 && width == npos && height == + /// npos) || (width != npos && height != npos) + /// + /// Will copy from (offset_x,offset_y) table of width by height. + /// Subtable must fully fit within table or fail. + /// Defaults will copy whole bittable, if any arguments are changed then + /// width/height must be specified (i.e. cannot be npos). + bool + push_copy( + bittable table, uint16_t offset_x = 0, uint16_t offset_y = 0, + uint16_t width = npos, uint16_t height = npos); + + /// @brief pushes a bittable to set, not copying contents, does not own + /// table memory + /// @param patch the patch to push + /// @return true on success, false otherwise + bool + push_ref(bittable patch); + + void + reset(); + + size_t + size() const noexcept + { + return patches_.size(); + } + bittable + get_patch(size_t i) const + { + return patches_.at(i); + } + std::span + get_patches() const noexcept + { + return patches_; + } + +protected: + std::pmr::monotonic_buffer_resource grid_res_; + std::vector patches_; +}; + +} // namespace warthog::manager + +#endif // WARTHOG_MANAGER_PATCH_SET_H diff --git a/include/warthog/manager/scenario_manager.h b/include/warthog/manager/scenario_manager.h new file mode 100644 index 0000000..633622d --- /dev/null +++ b/include/warthog/manager/scenario_manager.h @@ -0,0 +1,20 @@ +#ifndef WARTHOG_MANAGER_SCENARIO_MANAGER_H +#define WARTHOG_MANAGER_SCENARIO_MANAGER_H + +/// @file scenario_runner.h +/// +/// Alias for util::scenario_manager. +/// +/// @author: Ryan Hechenberger +/// @created: 2026-04-10 +/// + +#include "experiment.h" +#include + +namespace warthog::manager +{ +using warthog::util::scenario_manager; +} // namespace warthog::manager + +#endif // WARTHOG_MANAGER_SCENARIO_MANAGER_H diff --git a/include/warthog/manager/scenario_runner.h b/include/warthog/manager/scenario_runner.h new file mode 100644 index 0000000..8fbd0b9 --- /dev/null +++ b/include/warthog/manager/scenario_runner.h @@ -0,0 +1,173 @@ +#ifndef WARTHOG_MANAGER_SCENARIO_RUNNER_H +#define WARTHOG_MANAGER_SCENARIO_RUNNER_H + +/// @file scenario_runner.h +/// +/// Take a scenario_manager object and be able to progress through a dynamic +/// scenario. +/// +/// @author: Ryan Hechenberger +/// @created: 2026-04-09 +/// + +#include "grid_patch_set.h" +#include "scenario_manager.h" +#include + +namespace warthog::manager +{ + +struct patch_loc +{ + uint32_t patch_id; + uint16_t topleft_x; + uint16_t topleft_y; +}; + +class scenario_runner +{ +public: + scenario_runner(); + scenario_runner(const scenario_manager* scen); + ~scenario_runner(); + + /// @brief reset scenario to first command + void + clear(); + + /// @brief reset scenario to first command + void + restart(); + + /// @brief progress from current to count experiment away and return it + /// @param count + /// @return pair of the reached inst and snapshot id. + /// + /// Will progress through commands until count queries are encounted, + /// returning the final inst. When count == 1, is exactly the next inst. + /// All patches required + std::pair + experiment_next(uint32_t count = 1); + + /// @brief goto the start of the next snapshot (SNAPSHOT command) + /// @return snapshot id reached, or -1 if at end of commands (no more + /// snapshots) + /// + /// If current command is SNAPSHOT, if id != current_snapshot() then + /// already at next snapshot, otherwise progress until next SNAPSHOT + /// command is reached (or end of commands). Clears all patches from + /// get_patches() and replaces with any PATCH command to next snapshot. + int + snapshot_next(bool clear_patch = true); + + /// @brief starting at SNAPSHOT or current PATCH, apply all patches until + /// reaching SNAPSHOT or INST + /// @param clear_patch clears get_patches() + /// @return the number of patches, patch id are retrivable from + /// get_patches() + /// + /// Requires to be on SNAPSHOT or PATCH, otherwise returns 0 and does + /// nothing. If @clear_patch is set, clears get_patches(). Appends all + /// processed PATCH commands to get_patches(). + int + snapshot_patches(bool clear_patch = true); + + /// @brief returns the current inst experiment if at inst and progress to + /// next command + /// @return the current inst experiment if command is inst, otherwise + /// nullptr + /// + /// Requires to be on INST, otherwise return nullptr and do nothing. + /// If INST, returns corrisponding experiment and goto next command. + /// Does not affect get_patches(). + const experiment* + snapshot_inst(); + + /// @brief progress from current to count experiment away and return it + /// @param clear_patch clears get_patches() + /// @return pair of the reached inst and snapshot id. + /// + /// Will progress through commands until count queries are encounted, + /// returning the final inst. When count == 1, is exactly the next inst. + /// All patches required + std::span + snapshot_inst_all(); + + uint32_t + get_command_at() const noexcept + { + return command_at_; + } + + int32_t + get_experiment_at() const noexcept + { + return experiment_at_; + } + + int32_t + get_snapshot_at() const noexcept + { + return snapshot_at_; + } + + bool + complete() const noexcept + { + return command_at_ >= scenario_->get_commands().size(); + } + + std::span + get_patches() const noexcept + { + return patches_; + } + + size_t + mem() const noexcept + { + return 0; + } + + /// @brief setup grid and contain snapshot 0 + /// @param grid the gridmap to setup + /// @param patch_set the patch set to + /// @param setup_grid + /// @return true if operation is successful + bool + gridmap_init( + domain::gridmap& grid, const grid_patch_set& patch_set, + bool setup_grid = true); + + /// @brief apply patches from patch_set in order by get_patches() + /// @param grid grid to apply to + /// @param patch_set patch_set to pull patch data from + /// @return >=0 successful patches applied, <0 failed to apply negative id + /// (from 1) + int + gridmap_apply_patches( + domain::gridmap& grid, const grid_patch_set& patch_set); + + /// @brief apply a single patch to a gridmap + /// @param grid grid to apply to + /// @param patch patch bittable to copy + /// @param padded_x the padded topleft on grid + /// @param padded_y the padded topleft on grid + /// @return true if apply is successful, false otherwise + bool + girdmap_apply_patch( + domain::gridmap& grid, domain::gridmap::bittable patch, + uint32_t padded_x, uint32_t padded_y); + +protected: + const scenario_manager* scenario_ = nullptr; + std::vector patches_; + std::vector experiments_; + uint32_t command_at_ = 0; ///< command at, used for dynamic scenario + int32_t experiment_at_ = -1; + int32_t snapshot_at_ = -1; +}; + +} // namespace warthog::util + +#endif // WARTHOG_MANAGER_SCENARIO_RUNNER_H diff --git a/include/warthog/memory/bittable.h b/include/warthog/memory/bittable.h index b361898..23e1cc3 100644 --- a/include/warthog/memory/bittable.h +++ b/include/warthog/memory/bittable.h @@ -1,12 +1,13 @@ #ifndef WARTHOG_MEMORY_BITTABLE_H #define WARTHOG_MEMORY_BITTABLE_H +#include + #include #include #include #include #include -#include namespace warthog::memory { @@ -53,8 +54,7 @@ struct bittable_span_type /** * IdType: should be Identity, although integer is allowed. - * ValueBits: size of bits stored for each value. Must be power of 2 and - * smaller than BaseType. + * ValueBits: size of bits stored for each value. Must be power of 2 and <=8 * * bittable does not own its own data. * copy results in sharing underlying table data. @@ -65,8 +65,7 @@ struct bitarray { public: static_assert( - ValueBits <= sizeof(BaseType) * CHAR_BIT - && std::popcount(ValueBits) == 1, + ValueBits <= CHAR_BIT && std::popcount(ValueBits) == 1, "ValueBits must be to power of 2 and fit inside BaseType bits."); constexpr static size_t value_bits = ValueBits; using id_type = IdType; @@ -191,8 +190,8 @@ struct bitarray // return id position split // `m_data[first] >> second` will return the value as position id - constexpr std::pair - id_split(id_type id) const noexcept + constexpr static std::pair + id_split(id_type id) noexcept { id_value_type idval = id_value_type{id} << value_bit_width; return { @@ -238,6 +237,32 @@ struct bitarray } } + void + copy_p( + void* dest, uint8_t dest_bit, size_t count, id_type pos = id_type(0)) + { + // TODO: faster copy + assert(dest_bit < CHAR_BIT && (dest_bit & (ValueBits - 1)) == 0); + // primative implemtation + bitarray dest_t( + static_cast(dest)); + id_value_type p(pos); + id_value_type d = dest_bit; + while(count-- > 0) + { + auto v = get(id_type(p++)); + dest_t.set(typename decltype(dest_t)::id_type(d++), v); + } + } + void + copy( + bitarray dest, id_type dest_pos, size_t count, + id_type pos = id_type(0)) + { + auto [byte, bit] = id_split(dest_pos); + copy_p(dest.data() + byte, bit, count, pos); + } + /// /// struct data /// @@ -258,6 +283,7 @@ struct bittable : bitarray { public: using typename bittable::bitarray::id_type; + using typename bittable::bitarray::id_value_type; using typename bittable::bitarray::value_type; static constexpr size_t @@ -307,8 +333,7 @@ struct bittable : bitarray id_to_xy(id_type id) const noexcept { assert(m_dim.width != 0); - const auto value - = static_cast(id); + const auto value = static_cast(id); return { static_cast(value % m_dim.width), static_cast(value / m_dim.width)}; @@ -332,7 +357,7 @@ struct bittable : bitarray { return m_dim.width; } - /// @return width in bytes, rounded down + /// @return width in bytes constexpr uint32_t width_bytes() const noexcept { @@ -350,6 +375,11 @@ struct bittable : bitarray { return static_cast(m_dim.width) * m_dim.height; } + constexpr size_t + size_bytes() const noexcept + { + return sizeof(value_type) * data_elements(); + } constexpr bittable_dimension dim() const noexcept { @@ -420,6 +450,26 @@ struct bittable : bitarray return bittable::bitarray::id_split(id); } + void + copy( + bittable dest, id_type dest_pos, id_type pos, uint32_t width, + uint32_t height) + { + // TODO: faster copy + if(width == 0 || height == 0) return; + + id_value_type d(dest_pos); + id_value_type dd = dest.m_dim.width; + id_value_type p(pos); + id_value_type pd = m_dim.width; + do + { + bittable::bitarray::copy(dest, id_type(d), width, id_type(p)); + d += dd; + p += pd; + } while(--height != 0); + } + /// /// struct data /// diff --git a/include/warthog/search/search_node.h b/include/warthog/search/search_node.h index 90b9eba..bdfa9e4 100644 --- a/include/warthog/search/search_node.h +++ b/include/warthog/search/search_node.h @@ -16,23 +16,15 @@ namespace warthog::search { -class search_node +struct search_node { -public: - search_node(pad_id id = pad_id::max()) - : id_(id), parent_id_(warthog::SN_ID_MAX), g_(warthog::COST_MAX), - f_(warthog::COST_MAX), ub_(warthog::COST_MAX), status_(0), - priority_(warthog::INF32), search_number_(UINT32_MAX) - { - refcount_++; - } - - ~search_node() { refcount_--; } + search_node() noexcept = default; + search_node(pad_id id) noexcept : id_(id) { } inline void init( uint32_t search_number, pad_id parent_id, cost_t g, cost_t f, - cost_t ub = warthog::COST_MAX) + cost_t ub = warthog::COST_MAX) noexcept { parent_id_ = parent_id; f_ = f; @@ -43,103 +35,103 @@ class search_node } inline uint32_t - get_search_number() const + get_search_number() const noexcept { return search_number_; } inline void - set_search_number(uint32_t search_number) + set_search_number(uint32_t search_number) noexcept { search_number_ = search_number; } inline pad_id - get_id() const + get_id() const noexcept { return id_; } inline void - set_id(pad_id id) + set_id(pad_id id) noexcept { id_ = id; } inline bool - get_expanded() const + get_expanded() const noexcept { return status_; } inline void - set_expanded(bool expanded) + set_expanded(bool expanded) noexcept { status_ = expanded; } inline pad_id - get_parent() const + get_parent() const noexcept { return parent_id_; } inline void - set_parent(pad_id parent_id) + set_parent(pad_id parent_id) noexcept { parent_id_ = parent_id; } inline uint32_t - get_priority() const + get_priority() const noexcept { return priority_; } inline void - set_priority(uint32_t priority) + set_priority(uint32_t priority) noexcept { priority_ = priority; } inline cost_t - get_g() const + get_g() const noexcept { return g_; } inline void - set_g(cost_t g) + set_g(cost_t g) noexcept { g_ = g; } inline cost_t - get_f() const + get_f() const noexcept { return f_; } inline void - set_f(cost_t f) + set_f(cost_t f) noexcept { f_ = f; } inline cost_t - get_ub() const + get_ub() const noexcept { return ub_; } inline void - set_ub(cost_t ub) + set_ub(cost_t ub) noexcept { ub_ = ub; } inline void - relax(cost_t g, pad_id parent_id) + relax(cost_t g, pad_id parent_id) noexcept { assert(g < g_); f_ = (f_ - g_) + g; @@ -149,7 +141,7 @@ class search_node } inline bool - operator<(const search_node& other) const + operator<(const search_node& other) const noexcept { // static uint64_t SIGN_MASK = UINT64_MAX & (1ULL<<63); // cost_t result = this->f_ - other.f_; @@ -170,7 +162,7 @@ class search_node } inline bool - operator>(const search_node& other) const + operator>(const search_node& other) const noexcept { if(f_ > other.f_) { return true; } if(f_ < other.f_) { return false; } @@ -181,14 +173,14 @@ class search_node } inline bool - operator==(const search_node& other) const + operator==(const search_node& other) const noexcept { if(!(*this < other) && !(*this > other)) { return true; } return false; } inline bool - operator<=(const search_node& other) const + operator<=(const search_node& other) const noexcept { if(*this < other) { return true; } if(!(*this > other)) { return true; } @@ -196,50 +188,34 @@ class search_node } inline bool - operator>=(const search_node& other) const + operator>=(const search_node& other) const noexcept { if(*this > other) { return true; } if(!(*this < other)) { return true; } return false; } - inline void - print(std::ostream& out) const - { - out << "search_node id:" << get_id().id; - out << " p_id: "; - out << parent_id_.id; - out << " g: " << g_ << " f: " << this->get_f() << " ub: " << ub_ - << " expanded: " << get_expanded() << " " - << " search_number_: " << search_number_; - } + void + print(std::ostream& out) const; uint32_t - mem() + mem() noexcept { return sizeof(*this); } - static uint32_t - get_refcount() - { - return refcount_; - } - -private: - pad_id id_; - pad_id parent_id_; + pad_id id_ = pad_id(warthog::SN_ID_MAX); + pad_id parent_id_ = pad_id(warthog::SN_ID_MAX); - cost_t g_; - cost_t f_; - cost_t ub_; + cost_t g_ = warthog::COST_MAX; + cost_t f_ = warthog::COST_MAX; + cost_t ub_ = warthog::COST_MAX; // TODO steal the high-bit from priority instead of ::status_ ? - uint8_t status_; // open or closed - uint32_t priority_; // expansion priority + uint8_t status_ = 0; // open or closed + uint32_t priority_ = warthog::INF32; // expansion priority - uint32_t search_number_; - static uint32_t refcount_; + uint32_t search_number_ = UINT32_MAX; }; struct cmp_less_search_node diff --git a/include/warthog/util/experiment.h b/include/warthog/util/experiment.h index bfa76e6..db02007 100644 --- a/include/warthog/util/experiment.h +++ b/include/warthog/util/experiment.h @@ -23,7 +23,7 @@ #include #include -#include +#include namespace warthog::util { @@ -33,7 +33,14 @@ class experiment public: experiment( uint32_t sx, uint32_t sy, uint32_t gx, uint32_t gy, uint32_t mapwidth, - uint32_t mapheight, double d, std::string m) + uint32_t mapheight, double d, std::string_view m) + : startx_(sx), starty_(sy), goalx_(gx), goaly_(gy), + mapwidth_(mapwidth), mapheight_(mapheight), distance_(d), map_(m), + precision_(4) + { } + experiment( + double sx, double sy, double gx, double gy, uint32_t mapwidth, + uint32_t mapheight, double d, std::string_view m) : startx_(sx), starty_(sy), goalx_(gx), goaly_(gy), mapwidth_(mapwidth), mapheight_(mapheight), distance_(d), map_(m), precision_(4) @@ -49,24 +56,44 @@ class experiment uint32_t startx() const noexcept + { + return static_cast(startx_); + } + double + startx_f() const noexcept { return startx_; } uint32_t starty() const noexcept + { + return static_cast(starty_); + } + double + starty_f() const noexcept { return starty_; } uint32_t goalx() const noexcept + { + return static_cast(goalx_); + } + double + goalx_f() const noexcept { return goalx_; } uint32_t goaly() const noexcept + { + return static_cast(goaly_); + } + double + goaly_f() const noexcept { return goaly_; } @@ -77,7 +104,7 @@ class experiment return distance_; } - const std::string& + std::string_view map() const noexcept { return map_; @@ -114,15 +141,15 @@ class experiment get_instance() const noexcept { return search::problem_instance( - pack_id{starty_ * mapwidth_ + startx_}, - pack_id{goaly_ * mapwidth_ + goalx_}); + pack_id{starty() * mapwidth_ + startx()}, + pack_id{goaly() * mapwidth_ + goalx()}); } private: - uint32_t startx_, starty_, goalx_, goaly_; + double startx_, starty_, goalx_, goaly_; uint32_t mapwidth_, mapheight_; double distance_; - std::string map_; + std::string_view map_; int32_t precision_; }; diff --git a/include/warthog/util/gm_parser.h b/include/warthog/util/gm_parser.h index f96207b..613fdc6 100644 --- a/include/warthog/util/gm_parser.h +++ b/include/warthog/util/gm_parser.h @@ -9,6 +9,7 @@ // @created: 08/08/2012 // +#include #include #include #include @@ -20,31 +21,20 @@ namespace warthog::util class gm_header { public: - gm_header(unsigned int height, unsigned int width, const char* type) + gm_header(uint32_t height, uint32_t width, const char* type) : height_(height), width_(width), type_(type) { } - gm_header() : height_(0), width_(0), type_("") { } + gm_header() = default; - gm_header(const gm_header& other) { (*this) = other; } + ~gm_header() { } - virtual ~gm_header() { } - - gm_header& - operator=(const gm_header& other) - { - this->height_ = other.height_; - this->width_ = other.width_; - this->type_ = other.type_; - return *this; - } - - unsigned int height_; - unsigned int width_; + uint32_t height_; + uint32_t width_; std::string type_; }; -class gm_parser +class [[deprecated]] gm_parser { public: gm_parser(const char* filename); diff --git a/include/warthog/util/scenario_manager.h b/include/warthog/util/scenario_manager.h index 33ba408..d388f17 100644 --- a/include/warthog/util/scenario_manager.h +++ b/include/warthog/util/scenario_manager.h @@ -10,27 +10,75 @@ // (fields: bucket,map,mapwidth,mapheight,sx,sy,gx,gy,distance) // - DIMACS format (as at the 9th DIMACS Implementation Challenge) // (fields: q [source-id] [target-id]) +// - Dynamic format (tbd) // -// Supported formats for generate/write: -// - GPPC 1.0 format (as at 2012 Grid-based Path Planning Competition) -// -// @author: dharabor +// @author: dharabor & Ryan Hechenberger // @created: 21/08/2012 // #include "experiment.h" #include -#include -#include +#include +#include #include #include #include +#include #include namespace warthog::util { +struct scenario_command +{ + enum type_ : uint8_t + { + SNAPSHOT, + PATCH, + INST + }; + int type; ///< command type + int32_t bucket; ///< bucket id number (meta), snapshot id for dynamic + uint32_t + id; ///< SNAPSHOT: snapshot num, PATCH: patch to apply, INST: inst id + union cmd_ + { + struct snapshot_ + { + } snapshot; + struct patch_ + { + uint16_t topleft_x; + uint16_t topleft_y; + } patch; + struct inst_ + { + uint32_t experiment_id; ///< experiment number + } inst; + } cmd; ///< command union based on type + + static constexpr scenario_command + make_snapshot(int32_t bucket_id, uint32_t snapshot_id) noexcept + { + return scenario_command{ + SNAPSHOT, bucket_id, snapshot_id, {.snapshot = {}}}; + } + static constexpr scenario_command + make_patch( + int32_t bucket_id, uint32_t patch_id, uint16_t x, uint16_t y) noexcept + { + return scenario_command{PATCH, bucket_id, patch_id, {.patch = {x, y}}}; + } + static constexpr scenario_command + make_inst( + int32_t bucket_id, uint32_t inst_id, uint32_t experiment_id) noexcept + { + return scenario_command{ + INST, bucket_id, inst_id, {.inst = {experiment_id}}}; + } +}; + class scenario_manager { public: @@ -38,16 +86,50 @@ class scenario_manager ~scenario_manager(); experiment* - get_experiment(unsigned int which) + get_experiment(uint32_t which) { - if(which < experiments_.size()) { return experiments_[which]; } - return 0; + if(which >= experiments_.size()) + { + WARTHOG_GWARN_FMT( + "scenario has max {} experiments, cannot retrive {}", + experiments_.size(), which); + return nullptr; + } + return experiments_[which]; } const experiment* - get_experiment(unsigned int which) const + get_experiment(uint32_t which) const { - if(which < experiments_.size()) { return experiments_[which]; } - return 0; + if(which >= experiments_.size()) + { + WARTHOG_GWARN_FMT( + "scenario has max {} experiments, cannot retrive {}", + experiments_.size(), which); + return nullptr; + } + return experiments_[which]; + } + + std::span + get_experiments() noexcept + { + return experiments_; + } + std::span + get_experiments() const noexcept + { + return experiments_; + } + + std::span + get_commands() noexcept + { + return commands_; + } + std::span + get_commands() const noexcept + { + return commands_; } void @@ -68,37 +150,119 @@ class scenario_manager return sizeof(*this) + sizeof(experiment) * experiments_.size(); } - const std::string& - last_file_loaded() noexcept + std::string + last_file_loaded() const noexcept + { + return sfile_.string(); + } + const std::filesystem::path& + scenario_filename() const noexcept { return sfile_; } - void - clear() + const std::filesystem::path& + map_filename() const noexcept { - experiments_.clear(); + return mfile_; } + void + clear(); void - generate_experiments(domain::gridmap*, int num); + load_scenario(const std::filesystem::path& filelocation); void - load_scenario(const char* filelocation); + load_scenario( + std::istream& file, std::filesystem::path&& mapfile_override = {}); void write_scenario(std::ostream& out); + + std::string_view + get_cost_type() const noexcept + { + return cost_type_; + } + void + set_cost_type(std::string_view v) noexcept + { + cost_type_ = v; + } + void + set_cost_type(io::cost_type c) noexcept + { + cost_type_ = io::scenario_serialize::get_cost_str(c); + } + + bool + is_static_scenario() const noexcept + { + return static_scenario_start_ >= 0; + } + int32_t + get_static_scenario_start() const noexcept + { + return static_scenario_start_; + } + + uint32_t + get_scenario_width() const noexcept + { + return scenario_width_; + } void - sort(); // organise by increasing solution length + set_scenario_width(uint32_t width) noexcept + { + scenario_width_ = width; + } -private: + uint32_t + get_scenario_height() const noexcept + { + return scenario_height_; + } void - load_gppc_scenario(std::ifstream& infile); + set_scenario_height(uint32_t height) noexcept + { + scenario_height_ = height; + } + +protected: + std::errc + load_gppc_scenario(std::istream& scenfile); + + std::errc + load_gppc_scenario_body_v1(io::scenario_serialize& si); + std::errc + load_gppc_scenario_body_v2(io::scenario_serialize& si); + + std::string_view + copy_string(std::string_view str); + std::pmr::monotonic_buffer_resource experiments_res_; std::vector experiments_; - std::string sfile_; + std::vector commands_; + std::vector patches_; + std::filesystem::path sfile_; + std::filesystem::path mfile_; + std::string cost_type_; + io::scenario_version version_ = io::scenario_version::UNKNOWN; + uint32_t scenario_width_ = 0; + uint32_t scenario_height_ = 0; + uint32_t inst_count_ = 0; + uint32_t patch_count_ = 0; + int32_t static_scenario_start_ + = -1; ///< >=0: is static scenario where inst commands start at pos, + ///< else is dynamic scenario }; std::filesystem::path find_map_filename( - const scenario_manager& scenmgr, std::filesystem::path sfilename = {}); + const scenario_manager& scenmgr, + const std::filesystem::path& sfilename = {}); + +std::filesystem::path +find_map_filename( + const std::filesystem::path& scenmgr, + const std::filesystem::path& sfilename = {}); } // namespace warthog::util diff --git a/include/warthog/util/string.h b/include/warthog/util/string.h new file mode 100644 index 0000000..2419e4d --- /dev/null +++ b/include/warthog/util/string.h @@ -0,0 +1,505 @@ +#ifndef WARTHOG_UTIL_STRING_H +#define WARTHOG_UTIL_STRING_H + +// template.h +// +// Utility string functions. +// Includes handling of space and conversion of string to other values, +// mainly int/double. +// +// @author: Ryan Hechenberger +// @created: 2026-04-14 +// + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace warthog::util +{ + +/// @brief remove white spaces (as std::isspace) from beginning of string_view +inline std::string_view +ltrim(std::string_view str) +{ + str.remove_prefix( + std::find_if( + str.begin(), str.end(), + [](char c) { return !isspace((unsigned char)c); }) + - str.begin()); + return str; +} + +/// @brief remove white spaces (as std::isspace) from end of string_view +inline std::string_view +rtrim(std::string_view str) +{ + str.remove_suffix( + std::find_if( + str.rbegin(), str.rend(), + [](char c) { return !isspace((unsigned char)c); }) + - str.rbegin()); + return str; +} + +/// @brief string_view is either empty or only contains spaces (as +/// std::isspace) +inline bool +is_blank(std::string_view str) +{ + return std::all_of(str.begin(), str.end(), [](char c) { + return isspace((unsigned char)c); + }); +} + +struct token_return +{ + std::string_view token; ///< token extracted + size_t trim; ///< amount of space discarded from front of string +}; + +inline token_return +get_token(std::string_view str) +{ + auto fstr = ltrim(str); + if(fstr.empty()) { return {std::string_view(), str.size()}; } + auto endsp = std::find_if( + fstr.begin(), fstr.end(), + [](char c) { return isspace((unsigned char)c); }) + - fstr.begin(); + token_return ret; + ret.token = fstr.substr(0, endsp); + ret.trim = str.size() - fstr.size(); + return ret; +} + +inline token_return +get_token_quoted(std::string_view str, char quote = '"', char escape = '\\') +{ + if(quote == escape) { return {std::string_view(), 0}; } + auto fstr = ltrim(str); + if(fstr.empty()) { return {std::string_view(), str.size()}; } + token_return ret; + ret.trim = str.size() - fstr.size(); + if(fstr[0] == quote) + { + // quoted + size_t len = 0; + while(true) + { + len = fstr.find(quote, len + 1); + if(len == std::string_view::npos) + { + // no quote close + return {std::string_view(), ret.trim}; + } + auto epos = fstr.find_last_not_of(escape, len - 1) - len; + if((epos & 1) != 0) + { + // not delimited, return + ret.token = fstr.substr(0, len + 1); + break; + } + // delimited, continue + } + } + else + { + // no delimiter, read as normal + auto endsp = std::find_if( + fstr.begin(), fstr.end(), + [](char c) { return isspace((unsigned char)c); }) + - fstr.begin(); + ret.token = fstr.substr(0, endsp); + } + return ret; +} + +inline std::errc +parse_token(std::string_view token, std::integral auto& out, int base = 10) +{ + const char* const f = token.data(); + const char* const ed = token.data() + token.size(); + auto res = std::from_chars(token.data(), ed, out, base); + if(res.ec != std::errc{}) return res.ec; + // value set + if(res.ptr != ed) { return std::errc::invalid_argument; } + return std::errc{}; +} +inline std::errc +parse_token( + std::string_view token, std::floating_point auto& out, + std::chars_format fmt = std::chars_format::general) +{ + const char* const ed = token.data() + token.size(); + auto res = std::from_chars(token.data(), ed, out, fmt); + if(res.ec != std::errc{}) return res.ec; + // value set + if(res.ptr != ed) { return std::errc::invalid_argument; } + return std::errc{}; +} + +inline std::pair +parse_token_part( + std::string_view token, std::integral auto& out, int base = 10) +{ + std::from_chars_result res = std::from_chars( + token.data(), token.data() + token.size(), out, base); + if(res.ec != std::errc{}) return {{}, res.ec}; + // value set + token.remove_prefix(res.ptr - token.data()); + return {token, {}}; +} +inline std::pair +parse_token_part( + std::string_view token, std::floating_point auto& out, + std::chars_format fmt = std::chars_format::general) +{ + std::from_chars_result res + = std::from_chars(token.data(), token.data() + token.size(), out, fmt); + if(res.ec != std::errc{}) return {{}, res.ec}; + // value set + token.remove_prefix(res.ptr - token.data()); + return {token, {}}; +} + +inline std::pair +parse_token_quoted( + std::string_view token, char* buffer, size_t size, char quote = '"', + char escape = '\\') +{ + if(quote == escape) { return {nullptr, std::errc::invalid_argument}; } + if(token.empty() || size == 0) + { + return {nullptr, std::errc::invalid_argument}; + } + const char* const ed = token.end() + token.size(); + if(token[0] != '"') + { + // not quoted, take as whole token + if(std::any_of(token.data(), ed, [](char c) { + return std::isspace((unsigned char)c); + })) + { + // space detected, error + return {nullptr, std::errc::invalid_argument}; + } + if(token.size() + 1 > size) + { + // too large to hold token + return {nullptr, std::errc::value_too_large}; + } + token.copy(buffer, token.size()); + buffer[token.size()] = '\0'; + return {buffer + token.size() + 1, {}}; + } + // quoted text, process + const char* t_at = token.data() + 1; + while(true) + { + assert(size > 0); + if(t_at >= ed) + { + // reached end of token with no end-quote + return {buffer, std::errc::invalid_argument}; + } + if(*t_at == '\\') [[unlikely]] + { + if(++t_at >= ed) + { + // unexpected end of string + return {buffer, std::errc::invalid_argument}; + } + } + else if(*t_at == '"') [[unlikely]] + { + // found end quote, append null-terminator + *(buffer++) = '\0'; + // check is end of token + return { + buffer, + t_at + 1 == ed ? std::errc{} : std::errc::invalid_argument}; + } + *(buffer++) = *(t_at++); + if(--size == 0) + { + // out of space for null-terminator + return {buffer, std::errc::value_too_large}; + } + } +} +inline std::errc +parse_token_quoted( + std::string_view token, auto& str_out, char quote = '"', + char escape = '\\') +{ + str_out.resize(token.size() + 1); // plus 1 to hold \0 placed in buffer + auto [ed, ec] = parse_token_quoted( + token, str_out.data(), str_out.size(), quote, escape); + if(ec != std::errc{}) + { + // failed to read, clear str_out and return error code + str_out.resize(0); + return ec; + } + else + { + // successor, adjust str_out length and return + assert(ed - str_out.data() > 0); + str_out.resize((ed - str_out.data()) - 1); + return std::errc{}; + } +} + +class string_parser +{ +public: + string_parser(); + string_parser(std::string_view str) : m_str(str) { } + // error codes + void + clear_error() noexcept + { + m_error = std::errc{}; + } + std::errc + error() const noexcept + { + return m_error; + } + bool + is_error() const noexcept + { + return m_error != std::errc{}; + } + operator bool() const noexcept { return m_error == std::errc{}; } + + // set string + std::string_view + str() const noexcept + { + return m_str; + } + void + str(std::string_view s) noexcept + { + m_str = s; + m_token = std::string_view{}; + clear_error(); + } + + std::string_view + last_token() const noexcept + { + return m_token; + } + + string_parser& + next(std::integral auto& int_out, int base = 10) + { + if(is_error()) // do nothing if in error state + return *this; + if(!next_token_()) return *this; + std::errc ec = util::parse_token(m_token, int_out, base); + if(ec != std::errc{}) m_error = ec; + return *this; + } + string_parser& + next( + std::floating_point auto& float_out, + std::chars_format fmt = std::chars_format::general) + { + if(is_error()) // do nothing if in error state + return *this; + if(!next_token_()) return *this; + std::errc ec = util::parse_token(m_token, float_out, fmt); + if(ec != std::errc{}) m_error = ec; + return *this; + } + string_parser& + next(std::span& buffer) + { + if(is_error()) // do nothing if in error state + return *this; + if(buffer.empty()) + { + m_error = std::errc::invalid_argument; + return *this; + } + if(!next_token_()) return *this; + if(m_token.size() > buffer.size()) + { + m_error = std::errc::value_too_large; + return *this; + } + m_token.copy(buffer.data(), m_token.size()); + buffer = buffer.subspan(0, m_token.size()); + return *this; + } + string_parser& + next(std::string_view& out_token) + { + if(is_error()) // do nothing if in error state + return *this; + if(!next_token_()) return *this; + out_token = m_token; + return *this; + } + string_parser& + next(std::string& out_token) + { + if(is_error()) // do nothing if in error state + return *this; + if(!next_token_()) return *this; + out_token = m_token; + return *this; + } + string_parser& + next(std::pmr::string& out_token) + { + if(is_error()) // do nothing if in error state + return *this; + if(!next_token_()) return *this; + out_token = m_token; + return *this; + } + + string_parser& + next_q(std::span& buffer, char quote = '"', char escape = '\\') + { + if(is_error()) // do nothing if in error state + return *this; + if(buffer.empty()) + { + m_error = std::errc::invalid_argument; + return *this; + } + if(!next_token_quoted_(quote, escape)) return *this; + auto [ed, ec] = util::parse_token_quoted( + m_token, buffer.data(), buffer.size(), quote, escape); + if(ec != std::errc{}) + { + m_error = ec; + return *this; + } + if(ed == buffer.data()) + { + assert(false); // should never occur + m_error = std::errc::state_not_recoverable; + return *this; + } + buffer = buffer.subspan(0, (ed - buffer.data()) - 1); + return *this; + } + string_parser& + next_q(std::string& out_token, char quote = '"', char escape = '\\') + { + if(is_error()) // do nothing if in error state + return *this; + if(!next_token_quoted_(quote, escape)) return *this; + std::errc ec + = util::parse_token_quoted(m_token, out_token, quote, escape); + if(ec != std::errc{}) + { + m_error = ec; + return *this; + } + return *this; + } + string_parser& + next_q(std::pmr::string& out_token, char quote = '"', char escape = '\\') + { + if(is_error()) // do nothing if in error state + return *this; + if(!next_token_quoted_(quote, escape)) return *this; + std::errc ec + = util::parse_token_quoted(m_token, out_token, quote, escape); + if(ec != std::errc{}) + { + m_error = ec; + return *this; + } + return *this; + } + string_parser& + ignore() + { + if(is_error()) // do nothing if in error state + return *this; + next_token_(); + return *this; + } + string_parser& + ignore_q(char quote = '"', char escape = '\\') + { + if(is_error()) // do nothing if in error state + return *this; + next_token_quoted_(quote, escape); + return *this; + } + + bool + eof() + { + if(is_error()) return false; + if(!util::is_blank(m_str)) + { + m_error = std::errc::invalid_argument; + return false; + } + return true; + } + +protected: + bool + next_token_() + { + auto ret = util::get_token(m_str); + if(ret.token.empty()) + { + // failed to read a token, error + m_error = std::errc::invalid_argument; + return false; + } + else + { + m_token = ret.token; + assert(ret.trim <= m_str.size()); + m_str.remove_prefix(ret.token.size() + ret.trim); + return true; + } + } + bool + next_token_quoted_(char quote, char escape) + { + auto ret = util::get_token_quoted(m_str, quote, escape); + if(ret.token.empty()) + { + // failed to read a token, error + m_error = std::errc::invalid_argument; + return false; + } + else + { + m_token = ret.token; + assert(ret.trim <= m_str.size()); + m_str.remove_prefix(ret.token.size() + ret.trim); + return true; + } + } + +protected: + std::string_view m_str; + std::string_view m_token; + std::errc m_error = std::errc{}; +}; + +} // namespace warthog::util + +#endif // WARTHOG_UTIL_STRING_H diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6aebdc3..a5f752a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -9,6 +9,11 @@ geometry/geom.cpp io/grid.cpp io/grid_trace.cpp io/log.cpp +io/scenario.cpp +io/serialize_base.cpp + +manager/grid_patch_set.cpp +manager/scenario_runner.cpp memory/node_pool.cpp diff --git a/src/domain/gridmap.cpp b/src/domain/gridmap.cpp index 9f8ed5d..bdddcef 100644 --- a/src/domain/gridmap.cpp +++ b/src/domain/gridmap.cpp @@ -10,31 +10,102 @@ namespace warthog::domain { -gridmap::gridmap(unsigned int h, unsigned int w) : header_(h, w, "octile") +gridmap::gridmap() = default; + +gridmap::gridmap(uint32_t h, uint32_t w) : header_(h, w, "octile") { this->init_db(); } +gridmap::gridmap(std::istream& input) +{ + setup_stream_(input); +} + +gridmap::gridmap(io::bittable_serialize& parser) +{ + setup_ser_(parser); +} + +gridmap::gridmap(std::filesystem::path&& filename) +{ + filename_ = std::move(filename); + std::ifstream in(filename_); + setup_stream_(in); +} + +gridmap::gridmap(const std::filesystem::path& filename) + : gridmap(std::filesystem::path(filename)) +{ } + gridmap::gridmap(const char* filename) + : gridmap(std::filesystem::path(filename)) +{ } + +void +gridmap::setup(uint32_t h, uint32_t w) { - strcpy(filename_, filename); - io::bittable_serialize parser; + header_.height_ = h; + header_.width_ = w; + this->init_db(); +} + +void +gridmap::load(std::istream& input) +{ + setup_stream_(input); +} +void +gridmap::load(io::bittable_serialize& parser) +{ + setup_ser_(parser); +} +void +gridmap::load(std::filesystem::path&& filename) +{ + filename_ = std::move(filename); std::ifstream in(filename_); - if(!parser.read_header(in)) + setup_stream_(in); +} + +void +gridmap::load(const std::filesystem::path& filename) +{ + load(std::filesystem::path(filename)); +} + +void +gridmap::load(const char* filename) +{ + load(std::filesystem::path(filename)); +} + +void +gridmap::setup_stream_(std::istream& in) +{ + io::bittable_serialize parser; + parser.open_read(&in); + if(parser.read_header() != std::errc{}) + throw std::runtime_error("invalid grid format"); + setup_ser_(parser); +} + +void +gridmap::setup_ser_(io::bittable_serialize& parser) +{ + if(parser.read_grid_header() != std::errc{}) throw std::runtime_error("invalid grid format"); - if(parser.get_type() != io::bittable_type::OCTILE) - throw std::runtime_error("gridmap::gridmap must be OCTILE"); this->header_.type_ = "octile"; this->header_.width_ = parser.get_dim().width; this->header_.height_ = parser.get_dim().height; init_db(); - if(!parser.read_map(in, *this, 0, PADDED_ROWS)) + if(parser.read_grid_data(*this, 0, PADDED_ROWS) != std::errc{}) throw std::runtime_error("invalid grid format"); // calculate traversable num_traversable_ = static_cast(std::transform_reduce( - db_, db_ + db_size_, static_cast(0), std::plus(), - &std::popcount)); + db_.get(), db_.get() + db_size_, static_cast(0), + std::plus(), &std::popcount)); } void @@ -44,7 +115,7 @@ gridmap::init_db() // zeroes. this eliminates the need for bounds checking when // fetching the neighbours of a node. uint32_t store_width, store_height; - store_height = this->header_.height_ + PADDED_ROWS + PADDED_ROWS; + store_height = this->header_.height_ + 2 * PADDED_ROWS; // calculate # of extra/redundant padding bits required, // per row, to align map width with dbword size @@ -62,17 +133,14 @@ gridmap::init_db() this->db_size_ = bittable::calc_array_size(store_width, store_height) + 8; // create a one dimensional dbword array to store the grid - this->db_ = new warthog::dbword[db_size_]; - bittable::setup(this->db_, store_width, store_height); + this->db_ = std::make_unique(db_size_); + bittable::setup(this->db_.get(), store_width, store_height); fill(0); - max_id_ = this->dbheight_ * this->dbwidth_ - 1; + this->max_id_ = this->dbheight_ * this->dbwidth_ - 1; } -gridmap::~gridmap() -{ - delete[] db_; -} +gridmap::~gridmap() = default; void gridmap::print(std::ostream& out) diff --git a/src/io/grid.cpp b/src/io/grid.cpp index 0219240..f35f641 100644 --- a/src/io/grid.cpp +++ b/src/io/grid.cpp @@ -1,48 +1,159 @@ +#include + #include #include -#include namespace warthog::io { -bool -bittable_serialize::read_header(std::istream& in) +bittable_serialize::bittable_serialize() { - char buffer[32]; - auto read_key - = [&]() { return static_cast(in >> std::setw(32) >> buffer); }; + // increase max line length for larger grids + set_max_line_length(20 << 10); +} - if(!read_key()) return false; +std::errc +bittable_serialize::read_header(std::istream* in) +{ + std::errc err; + std::tie(in, err) = get_istream(in); + if(err != std::errc{}) { return err; } + std::string_view line; + std::string_view token; + + std::tie(line, err) = readline(in); + if(err != std::errc{}) { return err; } bittable_type detected_type = bittable_type::NONE; - if(std::strcmp(buffer, "type") == 0) { - if(!read_key()) return false; - if(std::strcmp(buffer, "octile") == 0) - detected_type = bittable_type::OCTILE; - else if(std::strcmp(buffer, "patch") == 0) - detected_type = bittable_type::PATCH; - else - detected_type = bittable_type::OTHER; - - if(!read_key()) return false; + parser par(line); + if(!par.next(token) || token != "type") { return std::errc::io_error; } + if(!par.next(token).eof()) { return std::errc::io_error; } } + if(token == "octile") + detected_type = bittable_type::OCTILE; + else if(token == "patch") + detected_type = bittable_type::PATCH; + else + detected_type = bittable_type::OTHER; + m_type = detected_type; + // read patch header before map header - if(detected_type == bittable_type::PATCH) + if(detected_type == bittable_type::OCTILE) { m_patch_amount = 1; } + else if(detected_type == bittable_type::PATCH) { - if(std::strcmp(buffer, "patches") != 0) return false; - in >> m_patch_count; - if(!read_key()) return false; + std::tie(line, err) = readline(in); + if(err != std::errc{}) { return err; } + parser par(line); + if(!par.next(token).next(m_patch_amount).eof()) + { + return std::errc::io_error; + } + if(token != "patches" || m_patch_amount > PATCH_COUNT_LIMIT) + { + m_patch_amount = 0; + return std::errc::argument_out_of_domain; + } } + m_patch_count = 0; + m_patch_id = 0; - if(std::strcmp(buffer, "height") != 0) return false; - if(!(in >> m_dim.height)) return false; - if(!read_key()) return false; - if(std::strcmp(buffer, "width") != 0) return false; - if(!(in >> m_dim.width)) return false; - m_type = detected_type; - if(!read_key() || std::strcmp(buffer, "map") != 0) return false; - return true; + return std::errc{}; +} + +std::errc +bittable_serialize::read_grid_header(std::istream* in) +{ + std::errc err; + std::tie(in, err) = get_istream(in); + if(err != std::errc{}) { return err; } + std::string_view line; + std::string_view token; + + m_patch_count += 1; + + // read height + std::tie(line, err) = readline(in); + if(err != std::errc{}) { return err; } + if(m_type == bittable_type::PATCH) + { + // get patch number + parser par(line); + if(!par.next(token).next(m_patch_id).eof()) + { + return std::errc::io_error; + } + if(token != "patch") { return std::errc::argument_out_of_domain; } + + std::tie(line, err) = readline(in); + if(err != std::errc{}) { return err; } + } + { + parser par(line); + if(!par.next(token).next(m_dim.height).eof()) + { + return std::errc::io_error; + } + if(token != "height" || m_dim.height > GRID_DIMENSION_MAX) + { + return std::errc::argument_out_of_domain; + } + } + // read width + std::tie(line, err) = readline(in); + if(err != std::errc{}) { return err; } + { + parser par(line); + if(!par.next(token).next(m_dim.width).eof()) + { + return std::errc::io_error; + } + if(token != "width" || m_dim.width > GRID_DIMENSION_MAX) + { + return std::errc::argument_out_of_domain; + } + } + // read "map" + std::tie(line, err) = readline(in); + if(err != std::errc{}) { return err; } + { + parser par(line); + if(!par.next(token).eof()) { return std::errc::io_error; } + if(token != "map") { return std::errc::argument_out_of_domain; } + } + + return std::errc{}; +} + +std::errc +bittable_serialize::read_grid_raw(std::span buffer, std::istream* in) +{ + const memory::bittable_dimension read_dim = m_dim; + if(buffer.size() < (uint64_t)read_dim.width * (uint64_t)read_dim.height) + { + return std::errc::result_out_of_range; + } + char* data_at = buffer.data(); + std::string_view token; + + std::errc err; + std::tie(in, err) = get_istream(in); + if(err != std::errc{}) { return err; } + std::string_view line; + for(uint32_t y = 0; y < read_dim.height; ++y) + { + // read row + std::tie(line, err) = readline(in); + if(err != std::errc{}) { return err; } + parser par(line); + if(!par.next(token).eof()) { return std::errc::io_error; } + if(token.size() != read_dim.width) + return std::errc::argument_out_of_domain; + // copy row to table + token.copy(data_at, token.size()); + data_at += read_dim.width; + } + return std::errc{}; } } // namespace warthog::io diff --git a/src/io/scenario.cpp b/src/io/scenario.cpp new file mode 100644 index 0000000..b6c0d53 --- /dev/null +++ b/src/io/scenario.cpp @@ -0,0 +1,685 @@ +#include + +#include +#include + +#include +#include +#include +#include + +namespace warthog::io +{ + +scenario_serialize::scenario_serialize() + : m_dyn_res(1024), m_string_res(256), m_cost_strings(&m_dyn_res), + m_cost_type(&m_dyn_res), m_cost_value(&m_dyn_res) +{ } +scenario_serialize::~scenario_serialize() = default; + +void +scenario_serialize::close() +{ + serialize_base::close(); + m_state = serialize_state::INIT; + m_version = scenario_version::UNKNOWN; + m_map_width = 0; + m_map_height = 0; + m_inst_at = 0; + m_cost_strings.clear(); + m_cost_type.clear(); + m_cost_value.clear(); +} + +void +scenario_serialize::set_relative_map_filename( + const std::filesystem::path& filename) +{ + m_map_filename + = std::filesystem::proximate(filename, get_scenario_filename()); +} + +int +scenario_serialize::last_command_type() const +{ + if(m_command_type == "Q") return CMD_INST; + if(m_command_type == "P") return CMD_PATCH; + return CMD_UNKNOWN; +} + +std::errc +scenario_serialize::read_version(std::istream* in) +{ + if(m_state != serialize_state::INIT) + { + return std::errc::state_not_recoverable; + } + std::errc err; + std::tie(in, err) = get_istream(in); + if(err != std::errc{}) + { + m_state = serialize_state::ERROR; + return err; + } + std::string_view line; + std::string_view token; + + std::tie(line, err) = readline(in); + if(err != std::errc{}) + { + m_state = serialize_state::ERROR; + return err; + } + parser par(line); + int version; + if(!par.next(token).next(version).eof()) + { + m_state = serialize_state::ERROR; + return par.error(); + } + if(version < 1 || version > 2) + { + m_state = serialize_state::ERROR; + return std::errc::invalid_argument; + } + m_version = version == 1 ? scenario_version::VERSION_1 + : scenario_version::VERSION_2; + m_state = serialize_state::VERSION; + return std::errc{}; +} + +std::errc +scenario_serialize::read_header(std::istream* in) +{ + if(!can_read(in) || m_state != serialize_state::VERSION) + { + return std::errc::state_not_recoverable; + } + + switch(m_version) + { + case scenario_version::VERSION_1: + return read_header_v1(in); + case scenario_version::VERSION_2: + return read_header_v2(in); + default: + return std::errc::state_not_recoverable; + } +} + +std::errc +scenario_serialize::read_header_v1(std::istream* in) +{ + std::errc ec; + bool s; + if(in == nullptr) + { + std::tie(in, ec) = get_istream(in); + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return ec; + } + } + // setup cost types + m_cost_strings.resize(1); + m_cost_type.resize(1); + m_cost_value.resize(1); + m_cost_strings[0] = get_cost_str(cost_type::G_8C_NCC); + m_cost_type[0] = cost_type::G_8C_NCC; + m_cost_value[0] = -1; + + // read first inst line to get map + auto [line, err] = readline(in); + if(err != std::errc{}) + { + m_state = serialize_state::ERROR; + return err; + } + parser par(line); + std::string_view map; + if(!par.ignore().next(map).next(m_map_width).next(m_map_height)) + { + m_state = serialize_state::ERROR; + return par.error(); + } + if(m_map_height < 1 || m_map_height > GRID_MAX_SIZE) + { + m_state = serialize_state::ERROR; + return std::errc::invalid_argument; + } + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return ec; + } + if(m_map_width <= 0 || m_map_width > GRID_MAX_SIZE || m_map_height <= 0 + || m_map_height > GRID_MAX_SIZE) + { + m_state = serialize_state::ERROR; + return std::errc::invalid_argument; + } + set_relative_map_filename(map); + unreadline(line); + m_state = serialize_state::COMMAND; + return std::errc{}; +} + +std::errc +scenario_serialize::read_header_v2(std::istream* in) +{ + std::errc err; + std::tie(in, err) = get_istream(in); + if(err != std::errc{}) + { + m_state = serialize_state::ERROR; + return err; + } + // read first inst line to get map + std::string_view line; + std::string_view token; + + // height + std::tie(line, err) = readline(in); + if(err != std::errc{}) + { + m_state = serialize_state::ERROR; + return err; + } + { + parser par(line); + if(!par.next(token).next(m_map_height).eof()) + { + m_state = serialize_state::ERROR; + return par.error(); + } + if(token != "height" || m_map_height < 1 + || m_map_height > GRID_MAX_SIZE) + { + m_state = serialize_state::ERROR; + return std::errc::invalid_argument; + } + } + + // width + std::tie(line, err) = readline(in); + if(err != std::errc{}) + { + m_state = serialize_state::ERROR; + return err; + } + { + parser par(line); + if(!par.next(token).next(m_map_width).eof()) + { + m_state = serialize_state::ERROR; + return par.error(); + } + if(token != "width" || m_map_width < 1 || m_map_width > GRID_MAX_SIZE) + { + m_state = serialize_state::ERROR; + return std::errc::invalid_argument; + } + } + + // cost + std::tie(line, err) = readline(in); + if(err != std::errc{}) + { + m_state = serialize_state::ERROR; + return err; + } + { + parser par(line); + int cost_count; + if(!par.next(token).next(cost_count)) + { + m_state = serialize_state::ERROR; + return par.error(); + } + if(token != "cost" || cost_count < 0 || cost_count > 128) + { + // limit cost_count to be within reason + m_state = serialize_state::ERROR; + return std::errc::invalid_argument; + } + m_cost_strings.resize(cost_count); + m_cost_type.resize(cost_count); + m_cost_value.resize(cost_count); + for(int i = 0; i < cost_count; ++i) + { + if(!par.next(token)) + { + m_state = serialize_state::ERROR; + return par.error(); + } + std::string_view ctype = copy_string(token); + m_cost_strings[i] = ctype; + m_cost_type[i] = get_cost_type(ctype); + m_cost_value[i] = -1; + } + // end of line + if(!par.eof()) + { + m_state = serialize_state::ERROR; + return par.error(); + } + } + + // get map (patch) filename + std::tie(line, err) = readline(in); + if(err != std::errc{}) + { + m_state = serialize_state::ERROR; + return err; + } + { + parser par(line); + std::string fname; + if(!par.next(token).next_q(fname).eof()) + { + m_state = serialize_state::ERROR; + return par.error(); + } + m_map_filename = std::move(fname); + if(token != "patch") + { + m_state = serialize_state::ERROR; + return std::errc::invalid_argument; + } + } + + // read in "commands" + std::tie(line, err) = readline(in); + if(err != std::errc{}) + { + m_state = serialize_state::ERROR; + return err; + } + { + parser par(line); + if(!par.next(token).eof()) + { + m_state = serialize_state::ERROR; + return par.error(); + } + if(token != "commands") + { + m_state = serialize_state::ERROR; + return std::errc::invalid_argument; + } + } + + // set_relative_map_filename(m_inst.map); + m_state = serialize_state::COMMAND; + return std::errc{}; +} + +std::pair +scenario_serialize::next_command_type(std::istream* in) +{ + if(!can_read(in) || m_state != serialize_state::COMMAND) + { + return {INVALID, std::errc::state_not_recoverable}; + } + std::errc err; + std::tie(in, err) = get_istream(in); + if(err != std::errc{}) + { + m_state = serialize_state::ERROR; + return {INVALID, err}; + } + + switch(m_version) + { + case scenario_version::VERSION_1: + { + auto [line, ec] = readline(in, true); + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return {INVALID, ec}; + } + if(istream_eof(in)) + { + m_state = serialize_state::END; + return {FINAL, {}}; + } + unreadline(line); + return {CMD_INST, {}}; + } + case scenario_version::VERSION_2: + { + auto [line, ec] = readline(in, true); + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return {INVALID, ec}; + } + if(istream_eof(in)) + { + m_state = serialize_state::END; + return {FINAL, {}}; + } + parser par(line); + if(!par.next(m_command_type)) { return {INVALID, par.error()}; } + auto cmd_type = last_command_type(); + unreadline(line); + return {cmd_type, {}}; + } + default: + return {INVALID, std::errc::state_not_recoverable}; + } +} + +std::errc +scenario_serialize::skip_commands(int count, std::istream* in) +{ + if(m_state == serialize_state::END) { return {}; } + if(!can_read(in) || m_state != serialize_state::COMMAND) + { + return std::errc::state_not_recoverable; + } + + std::errc ec; + std::string_view line; + std::tie(in, ec) = get_istream(in); + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return ec; + } + while(count > 0) + { + --count; + std::tie(line, ec) = readline(in, true); + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return ec; + } + if(istream_eof(in)) + { + m_state = serialize_state::END; + break; + } + } + return {}; +} + +std::pair +scenario_serialize::read_instance_line( + scenario_instance& inst, std::istream* in) +{ + if(m_state == serialize_state::END) { return {FINAL, {}}; } + if(!can_read(in) || m_state != serialize_state::COMMAND) + { + return {0, std::errc::state_not_recoverable}; + } + + switch(m_version) + { + case scenario_version::VERSION_1: + return read_instance_line_v1(inst, in); + case scenario_version::VERSION_2: + return read_instance_line_v2(inst, in); + default: + return {INVALID, std::errc::state_not_recoverable}; + } +} + +std::pair +scenario_serialize::read_instance_line_v1( + scenario_instance& inst, std::istream* in) +{ + assert(can_read(in)); + // move to start of read + std::errc ec; + std::string_view line; + std::string_view token; + + std::tie(in, ec) = get_istream(in); + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return {INVALID, ec}; + } + std::tie(line, ec) = readline(in, true); + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return {INVALID, ec}; + } + if(istream_eof(in)) + { + m_state = serialize_state::END; + return {FINAL, std::errc{}}; + } + parser par(line); + if(!par.next(inst.bucket) + .next(token) + .next(inst.width) + .next(inst.height) + .next(inst.start_x) + .next(inst.start_y) + .next(inst.goal_x) + .next(inst.goal_y) + .next(m_cost_value.at(0)) + .eof()) + { + // state not set to error, up to user + return {INVALID, par.error()}; + } + inst.map = token; + inst.cost = m_cost_value; + if(!std::isfinite(inst.start_x) || !std::isfinite(inst.start_y) + || !std::isfinite(inst.goal_x) || !std::isfinite(inst.goal_y) + || !std::isfinite(inst.cost[0])) + { + return {INVALID, std::errc{}}; + } + // perform checking of values, but do not return as error if fail + if(inst.width != m_map_width || inst.height != m_map_height) + { + return {INVALID, std::errc{}}; + } + if(m_force_int) + { + // check integer bounds + if(rint(inst.start_x) != inst.start_x + || rint(inst.start_y) != inst.start_y + || rint(inst.goal_x) != inst.goal_x + || rint(inst.goal_y) != inst.goal_y) + { + return {INVALID, std::errc{}}; + } + if(inst.start_x < 0 || inst.start_y >= m_map_width || inst.goal_x < 0 + || inst.goal_y >= m_map_height) + { + return {INVALID, std::errc{}}; + } + } + else + { + if(inst.start_x < 0 || inst.start_y > m_map_width || inst.goal_x < 0 + || inst.goal_y > m_map_height) + { + return {INVALID, std::errc{}}; + } + } + return {VALID, std::errc{}}; +} + +std::pair +scenario_serialize::read_instance_line_v2( + scenario_instance& inst, std::istream* in) +{ + assert(can_read(in)); + // move to start of read + std::errc ec; + std::string_view line; + std::string_view token; + + std::tie(in, ec) = get_istream(in); + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return {INVALID, ec}; + } + std::tie(line, ec) = readline(in, true); + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return {INVALID, ec}; + } + if(istream_eof(in)) + { + m_state = serialize_state::END; + return {FINAL, std::errc{}}; + } + + parser par(line); + if(!par.next(m_command_type)) { return {INVALID, par.error()}; } + auto cmd_type = last_command_type(); + if(cmd_type != CMD_INST) + { + unreadline(line); + return {cmd_type, std::errc{}}; + } + if(!par.next(inst.bucket) + .next(inst.start_x) + .next(inst.start_y) + .next(inst.goal_x) + .next(inst.goal_y)) + { + return {INVALID, par.error()}; + } + if(!std::isfinite(inst.start_x) || !std::isfinite(inst.start_y) + || !std::isfinite(inst.goal_x) || !std::isfinite(inst.goal_y)) + { + return {INVALID, std::errc::invalid_argument}; + } + inst.width = inst.height = 0; + for(auto& cost : m_cost_value) + { + if(!par.next(cost)) { return {INVALID, par.error()}; } + if(!std::isfinite(cost)) + { + return {INVALID, std::errc::invalid_argument}; + } + } + inst.cost = m_cost_value; + if(!par.eof()) + { + // unexpected number of paramters + return {INVALID, par.error()}; + } + if(m_force_int) + { + // check integer bounds + if(rint(inst.start_x) != inst.start_x + || rint(inst.start_y) != inst.start_y + || rint(inst.goal_x) != inst.goal_x + || rint(inst.goal_y) != inst.goal_y) + { + return {INVALID, std::errc{}}; + } + if(inst.start_x < 0 || inst.start_y >= m_map_width || inst.goal_x < 0 + || inst.goal_y >= m_map_height) + { + return {INVALID, std::errc{}}; + } + } + else + { + if(inst.start_x < 0 || inst.start_y > m_map_width || inst.goal_x < 0 + || inst.goal_y > m_map_height) + { + return {INVALID, std::errc{}}; + } + } + return {VALID, std::errc{}}; +} + +std::pair +scenario_serialize::read_patch_line(scenario_patch& patch, std::istream* in) +{ + if(!can_read(in) || m_state != serialize_state::COMMAND) + { + return {0, std::errc::state_not_recoverable}; + } + + switch(m_version) + { + case scenario_version::VERSION_2: + return read_patch_line_v2(patch, in); + default: + return {0, std::errc::state_not_recoverable}; + } +} + +auto +scenario_serialize::read_patch_line_v2(scenario_patch& patch, std::istream* in) + -> std::pair +{ + assert(can_read(in)); + // move to start of read + std::errc ec; + std::string_view line; + std::string_view token; + + std::tie(in, ec) = get_istream(in); + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return {INVALID, ec}; + } + std::tie(line, ec) = readline(in, true); + if(ec != std::errc{}) + { + m_state = serialize_state::ERROR; + return {INVALID, ec}; + } + if(istream_eof(in)) + { + m_state = serialize_state::END; + return {FINAL, std::errc{}}; + } + + parser par(line); + if(!par.next(token)) { return {INVALID, par.error()}; } + auto cmd_type = last_command_type(); + if(cmd_type != CMD_PATCH) + { + unreadline(line); + return {cmd_type, std::errc{}}; + } + + if(!par.next(patch.bucket) + .next(patch.patch_id) + .next(patch.loc_x) + .next(patch.loc_y) + .eof()) + { + return {INVALID, par.error()}; + } + if(patch.loc_x >= m_map_width || patch.loc_y > m_map_height) + { + return {INVALID, {}}; + } + + return {VALID, std::errc{}}; +} + +std::string_view +scenario_serialize::copy_string(std::string_view str) +{ + size_t len = str.size(); + if(len == 0) return std::string_view(); + char* data = static_cast(m_string_res.allocate(len + 1, 8)); + std::memcpy(data, str.data(), len); + data[len] = '\0'; + return std::string_view(data, len); +} + +} // namespace warthog::io diff --git a/src/io/serialize_base.cpp b/src/io/serialize_base.cpp new file mode 100644 index 0000000..7e59900 --- /dev/null +++ b/src/io/serialize_base.cpp @@ -0,0 +1,165 @@ +#include + +#include +#include +#include +#include +#include + +namespace warthog::io +{ + +serialize_base::serialize_base() { } +serialize_base::~serialize_base() = default; + +std::errc +serialize_base::open_read(std::istream* is) +{ + close(); + if(is != nullptr) { m_stream_in = is; } + else + { + auto stream = std::make_unique(get_filename()); + m_stream_in = stream.get(); + m_stream = std::move(stream); + } + if(!*m_stream_in) + { + // bad stream + m_stream_in = nullptr; + m_stream = nullptr; + return std::errc::io_error; + } + return std::errc{}; +} + +std::errc +serialize_base::open_write(std::ostream* os) +{ + close(); + if(os != nullptr) { m_stream_out = os; } + else + { + auto stream = std::make_unique(get_filename()); + m_stream_out = stream.get(); + m_stream = std::move(stream); + } + if(!*m_stream_out) + { + // bad stream + m_stream_out = nullptr; + m_stream = nullptr; + return std::errc::io_error; + } + return std::errc{}; +} + +void +serialize_base::close() +{ + m_line_num = 0; + m_line = std::string_view(); + m_unget_line.clear(); + m_stream = nullptr; + m_stream_in = nullptr; + m_stream_out = nullptr; +} + +bool +serialize_base::istream_eof(std::istream* in) +{ + auto [s, err] = get_istream(in); + if(err != std::errc{}) return false; + return s->eof(); +} + +std::pair +serialize_base::readline(std::istream* in, bool skip_blanks) +{ + size_t len; + auto [s, err] = get_istream(in); + if(!m_line_data) + { + m_max_line_length + = m_max_line_length > 0 ? m_max_line_length : max_line_length; + m_line_data = std::make_unique(m_max_line_length); + } + while(true) + { + if(len = m_unget_line.size(); len != 0) + { + // return last unreadline + if(len > m_max_line_length - 1) + { + return {{}, std::errc::invalid_argument}; + } + if(len == 1 && m_unget_line[0] == '\0') + { + // empty line + m_line = std::string_view(); + } + else + { + // copy line + std::memcpy(m_line_data.get(), m_unget_line.c_str(), len + 1); + m_line = std::string_view(m_line_data.get(), len); + } + m_unget_line.clear(); + } + else + { + if(err != std::errc{}) { return {{}, err}; } + if(s->eof()) { return {{}, std::errc::io_error}; } + if(!s->getline(m_line_data.get(), m_max_line_length)) + { + if(s->eof() && s->gcount() == 0) + { + s->clear(std::ios::eofbit); + // extracted no characters and reached end of file, return + // no error + return {{}, {}}; + } + else + { + // error + return {{}, std::errc::io_error}; + } + } + // get length, if eof is set, is last line and no delimiter was + // extracted + len = static_cast(s->gcount()) - (s->eof() ? 0 : 1); + if(len == 0) { m_line = std::string_view(); } + else + { + if(std::iscntrl(m_line_data[len - 1])) + { + // mainly \r reading windows files in linux + m_line_data[--len] = '\0'; + } + m_line = std::string_view(m_line_data.get(), len); + } + m_line_num += 1; + } + if(skip_blanks && is_line_blank(m_line)) + { + continue; // blank line, repeat + } + return {m_line, {}}; + } +} +void +serialize_base::unreadline(std::string_view line) +{ + if(!line.empty()) [[likely]] { m_unget_line = line; } + else { m_unget_line = '\0'; } +} + +bool +serialize_base::is_line_blank(std::string_view line) +{ + return std::all_of(line.begin(), line.end(), [](char c) { + return std::isspace((unsigned char)c); + }); +} + +} // namespace warthog::io diff --git a/src/manager/grid_patch_set.cpp b/src/manager/grid_patch_set.cpp new file mode 100644 index 0000000..111bfe8 --- /dev/null +++ b/src/manager/grid_patch_set.cpp @@ -0,0 +1,186 @@ +#include + +#include +#include + +#include + +namespace warthog::manager +{ + +bool +grid_patch_set::load(std::istream& file, int max_grids) +{ + io::bittable_serialize S; + if(auto ec = S.open_read(&file); ec != std::errc{}) + { + WARTHOG_GWARN_FMT("grid patch failed to open file code={}", (int)ec); + return false; + } + if(int r = deserialize(S, max_grids); r < 0) + { + WARTHOG_GWARN_FMT("grid patch failed to read patch {}", -r - 1); + return false; + } + return true; +} + +bool +grid_patch_set::load(const std::filesystem::path& maps, int max_grids) +{ + io::bittable_serialize S; + S.set_filename(std::filesystem::path(maps)); + if(auto ec = S.open_read(); ec != std::errc{}) + { + WARTHOG_GWARN_FMT("grid patch failed to open file code={}", (int)ec); + return false; + } + if(int r = deserialize(S); r < 0) + { + WARTHOG_GWARN_FMT("grid patch failed to read patch {}", -r - 1); + return false; + } + return true; +} + +int +grid_patch_set::deserialize( + io::bittable_serialize& S, int max_grids, uint32_t flags) +{ + // read header + if(S.get_type() == io::bittable_type::NONE) + { + // header not read + if((flags & SKIP_HEADER) != 0) + { + WARTHOG_GWARN("grid patch SKIP_HEADER for serializer when header " + "has not been read."); + return -1; + } + if(auto ec = S.read_header(); ec != std::errc{}) + { + WARTHOG_GWARN_FMT( + "grid patch failed to read header code={}", (int)ec); + return -1; + } + } + else + { + // header has been read, check + if((flags & FORCE_HEADER) != 0) + { + WARTHOG_GWARN("grid patch FORCE_HEADER for serializer when header " + "has already been read."); + return -1; + } + } + auto type = S.get_type(); + if(type != io::bittable_type::OCTILE && type != io::bittable_type::PATCH) + { + WARTHOG_GWARN( + "grid patch only supports reading type octile or patch."); + return -1; + } + + // start reading grids + int count = 0; + if(max_grids < 0) { max_grids = std::numeric_limits::max(); } + while(count < max_grids && S.get_patch_count() < S.get_patch_amount()) + { + // read new grid + ++count; + if(auto ec = S.read_grid_header(); ec != std::errc{}) + { + WARTHOG_GWARN_FMT( + "grid patch failed to read patch {} header code={}", count, + (int)ec); + return -count; + } + if((flags & IGNORE_INDEX) == 0) + { + // check index + if(S.get_patch_id() != patches_.size()) + { + // index mismatch + WARTHOG_GWARN_FMT( + "grid patch failed patch {} index mismatch of {} expected " + "{}", + count, S.get_patch_id(), patches_.size()); + return -count; + } + } + // setup patch data + auto dim = S.get_dim(); + auto bytes = bittable::calc_array_size(dim.width, dim.height); + auto* grid_data = static_cast( + grid_res_.allocate(bytes, alignof(bittable::value_type))); + bittable patch(grid_data, dim.width, dim.height); + if(auto ec = S.read_grid_data(patch); ec != std::errc{}) + { + WARTHOG_GWARN_FMT( + "grid patch failed to read patch {} data code={}", count, + (int)ec); + return -count; + } + patches_.push_back(patch); + } + + return count; +} + +bool +grid_patch_set::push_copy( + bittable table, uint16_t offset_x, uint16_t offset_y, uint16_t width, + uint16_t height) +{ + if(table.size() == 0) { return false; } + bittable patch; + if(offset_x == 0 && offset_y == 0 && width == npos && height == npos) + { + // copy as is + auto size = table.size_bytes(); + auto* grid_data = static_cast( + grid_res_.allocate(size, alignof(bittable::value_type))); + std::memcpy(grid_data, table.data(), size); + patch = bittable(grid_data, table.width(), table.height()); + } + else + { + // crop + if(offset_x >= table.width() || width > table.width() + || offset_x + width > table.width()) + { + return false; + } + if(offset_y >= table.height() || height > table.height() + || offset_y + height > table.height()) + { + return false; + } + auto size = bittable::calc_array_size(width, height); + auto* grid_data = static_cast( + grid_res_.allocate(size, alignof(bittable::value_type))); + patch = bittable(grid_data, table.width(), table.height()); + table.copy( + patch, pad_id::zero(), table.xy_to_id(offset_x, offset_y), width, + height); + } + patches_.push_back(patch); + return true; +} + +bool +grid_patch_set::push_ref(bittable patch) +{ + patches_.push_back(patch); + return true; +} + +void +grid_patch_set::reset() +{ + patches_.clear(); + grid_res_.release(); +} + +} // namespace warthog::manager diff --git a/src/manager/scenario_runner.cpp b/src/manager/scenario_runner.cpp new file mode 100644 index 0000000..724dd3d --- /dev/null +++ b/src/manager/scenario_runner.cpp @@ -0,0 +1,250 @@ +#include + +#include +#include +#include + +#include +#include + +namespace warthog::manager +{ + +scenario_runner::scenario_runner() = default; + +scenario_runner::scenario_runner(const scenario_manager* scen) + : scenario_(scen) +{ } + +scenario_runner::~scenario_runner() = default; + +std::pair +scenario_runner::experiment_next(uint32_t count) +{ + assert(scenario_ != nullptr); + auto commands = scenario_->get_commands(); + patches_.clear(); // reset patches + if(count == 0) return {nullptr, 0}; + int patch_count = 0; + while(command_at_ < commands.size()) + { + auto cmd = commands[command_at_]; + // command_at_ incremented in following fuction calls + switch(cmd.type) + { + case util::scenario_command::SNAPSHOT: + case util::scenario_command::PATCH: + patch_count += snapshot_patches(); + break; + case util::scenario_command::INST: + if(const experiment* inst = snapshot_inst(); inst != nullptr) + { + if(--count == 0) return {inst, patch_count}; + } + break; + default: + // should never be reached + ++command_at_; + WARTHOG_GDEBUG("scenario_runner::experiment_next invalid command " + "type in " WARTHOG_FILENAME_LINE); + } + // exits loop + } + // no more experiments + return {nullptr, patch_count}; +} + +void +scenario_runner::clear() +{ + restart(); + scenario_ = nullptr; +} + +void +scenario_runner::restart() +{ + patches_.clear(); + experiments_.clear(); + command_at_ = 0; + experiment_at_ = -1; + snapshot_at_ = -1; +} + +int +scenario_runner::snapshot_next(bool clear_patch) +{ + assert(scenario_ != nullptr); + auto commands = scenario_->get_commands(); + if(clear_patch) patches_.clear(); + while(command_at_ < commands.size()) + { + auto cmd = commands[command_at_]; + switch(cmd.type) + { + case util::scenario_command::SNAPSHOT: + if(cmd.id != snapshot_at_) + { + // at new snapshot, return + snapshot_at_ = cmd.id; + return snapshot_at_; + } + // current snapshot, goto next snapshot + [[fallthrough]]; + case util::scenario_command::INST: + ++experiment_at_; + break; + case util::scenario_command::PATCH: + snapshot_patches(false); + // snapshot_patches increments snapshot_at_ + break; + default: + WARTHOG_GDEBUG("scenario_runner::snapshot_next invalid command " + "type in " WARTHOG_FILENAME_LINE); + } + } + return false; +} + +int +scenario_runner::snapshot_patches(bool clear_patch) +{ + assert(scenario_ != nullptr); + auto commands = scenario_->get_commands(); + if(clear_patch) patches_.clear(); + int count = 0; + // if start of snapshot, apply that snapshot patches + if(command_at_ < commands.size() + && commands[command_at_].type == util::scenario_command::SNAPSHOT) + { + snapshot_at_ = commands[command_at_].id; + command_at_ += 1; + } + // while command_at_ is PATCH, add patch to applied list + while(command_at_ < commands.size()) + { + if(auto cmd = commands[command_at_]; + cmd.type == util::scenario_command::PATCH) + { + command_at_ += 1; + count += 1; + patches_.push_back( + {cmd.id, cmd.cmd.patch.topleft_x, cmd.cmd.patch.topleft_y}); + } + else { break; } + } + // end at first non-PATCH command + return count; +} + +const experiment* +scenario_runner::snapshot_inst() +{ + assert(scenario_ != nullptr); + auto commands = scenario_->get_commands(); + if(command_at_ >= commands.size()) return nullptr; + auto cmd = commands[command_at_]; + if(cmd.type != util::scenario_command::INST) return nullptr; + command_at_ += 1; + experiment_at_ += 1; + if(cmd.cmd.inst.experiment_id >= scenario_->num_experiments() + || cmd.cmd.inst.experiment_id != (uint32_t)experiment_at_) + { + WARTHOG_GERROR_FMT( + "scenario_runner::snapshot_inst invalid experiment_id {} to " + "experiment, expected {} (max {}) in {}", + cmd.cmd.inst.experiment_id, experiment_at_, + scenario_->num_experiments(), WARTHOG_FILENAME_LINE); + return nullptr; + } + return scenario_->get_experiment(cmd.cmd.inst.experiment_id); +} + +std::span +scenario_runner::snapshot_inst_all() +{ + assert(scenario_ != nullptr); + experiments_.clear(); + auto commands = scenario_->get_commands(); + auto exp = scenario_->get_experiments(); + while(command_at_ < commands.size()) + { + auto cmd = commands[command_at_]; + if(cmd.type != util::scenario_command::INST) break; + if(cmd.cmd.inst.experiment_id >= exp.size() + || cmd.cmd.inst.experiment_id != (uint32_t)experiment_at_) + { + WARTHOG_GERROR_FMT( + "scenario_runner::snapshot_inst_all invalid experiment_id {} " + "to experiment, expected {} (max {}) in {}", + cmd.cmd.inst.experiment_id, experiment_at_, exp.size(), + WARTHOG_FILENAME_LINE); + return {}; + } + experiments_.push_back(exp[cmd.cmd.inst.experiment_id]); + command_at_ += 1; + experiment_at_ += 1; + } + return experiments_; +} + +bool +scenario_runner::gridmap_init( + domain::gridmap& grid, const grid_patch_set& patch_set, bool setup_grid) +{ + if(setup_grid) + { + grid.setup( + scenario_->get_scenario_height(), scenario_->get_scenario_width()); + // grid is 0 (non-traversable), make map area traversable but keep + // padding non-traversable + for(uint32_t y = 0, ye = grid.header_height(), + xe = grid.header_width(); + y < ye; ++y) + { + pad_id row_id = grid.to_padded_id_from_unpadded(0, y); + for(uint32_t x = 0; x < xe; ++x, ++row_id.id) + { + grid.set_label(row_id, true); + } + } + } + restart(); + snapshot_patches(); + return gridmap_apply_patches(grid, patch_set) >= 0; +} +int +scenario_runner::gridmap_apply_patches( + domain::gridmap& grid, const grid_patch_set& patch_set) +{ + int count = 0; + for(auto& P : patches_) + { + ++count; + if(P.patch_id >= patch_set.size()) return -count; + uint32_t x, y; + grid.to_padded_xy_from_unpadded(P.topleft_x, P.topleft_y, x, y); + if(!girdmap_apply_patch(grid, patch_set.get_patch(P.patch_id), x, y)) + { + return -count; + } + } + return count; +} +bool +scenario_runner::girdmap_apply_patch( + domain::gridmap& grid, domain::gridmap::bittable patch, uint32_t padded_x, + uint32_t padded_y) +{ + if((uint64_t)padded_x + patch.width() >= (uint64_t)grid.width() + || (uint64_t)padded_y + patch.height() >= (uint64_t)grid.height()) + return false; + + // apply patch + patch.copy( + grid, grid.to_padded_id_from_padded(padded_x, padded_y), + pad_id::zero(), patch.width(), patch.height()); + return true; +} + +} // namespace warthog::util diff --git a/src/search/gridmap_expansion_policy.cpp b/src/search/gridmap_expansion_policy.cpp index f60470b..676e2ed 100644 --- a/src/search/gridmap_expansion_policy.cpp +++ b/src/search/gridmap_expansion_policy.cpp @@ -1,4 +1,5 @@ #include + #include #include diff --git a/src/search/search_node.cpp b/src/search/search_node.cpp index 4e90feb..cae2985 100644 --- a/src/search/search_node.cpp +++ b/src/search/search_node.cpp @@ -1,6 +1,18 @@ #include -unsigned int warthog::search::search_node::refcount_ = 0; +namespace warthog::search +{ + +void +search_node::print(std::ostream& out) const +{ + out << "search_node id:" << get_id().id; + out << " p_id: "; + out << parent_id_.id; + out << " g: " << g_ << " f: " << this->get_f() << " ub: " << ub_ + << " expanded: " << get_expanded() << " " + << " search_number_: " << search_number_; +} std::ostream& operator<<(std::ostream& str, const warthog::search::search_node& sn) @@ -9,3 +21,5 @@ operator<<(std::ostream& str, const warthog::search::search_node& sn) return str; } + +} // namespace warthog::search diff --git a/src/search/vl_gridmap_expansion_policy.cpp b/src/search/vl_gridmap_expansion_policy.cpp index e8750c4..0a7945d 100644 --- a/src/search/vl_gridmap_expansion_policy.cpp +++ b/src/search/vl_gridmap_expansion_policy.cpp @@ -1,5 +1,6 @@ -#include #include + +#include #include namespace warthog::search diff --git a/src/util/dimacs_parser.cpp b/src/util/dimacs_parser.cpp index 51eb8b5..75b0f64 100644 --- a/src/util/dimacs_parser.cpp +++ b/src/util/dimacs_parser.cpp @@ -1,6 +1,7 @@ -#include #include +#include + #include #include #include diff --git a/src/util/helpers.cpp b/src/util/helpers.cpp index f3c78b2..518176b 100644 --- a/src/util/helpers.cpp +++ b/src/util/helpers.cpp @@ -1,6 +1,7 @@ +#include + #include #include -#include #include #include diff --git a/src/util/scenario_manager.cpp b/src/util/scenario_manager.cpp index 7bbaa13..29a7f64 100644 --- a/src/util/scenario_manager.cpp +++ b/src/util/scenario_manager.cpp @@ -1,6 +1,8 @@ +#include + +#include #include #include -#include #include #include @@ -10,72 +12,285 @@ namespace warthog::util scenario_manager::scenario_manager() { } -scenario_manager::~scenario_manager() +scenario_manager::~scenario_manager() = default; + +void +scenario_manager::clear() { - for(unsigned int i = 0; i < experiments_.size(); i++) - { - delete experiments_[i]; - } experiments_.clear(); + experiments_res_.release(); + mfile_.clear(); } void -scenario_manager::load_scenario(const char* filelocation) +scenario_manager::load_scenario(const std::filesystem::path& filelocation) { - std::ifstream infile; - infile.open(filelocation, std::ios::in); + sfile_ = filelocation; + std::ifstream in(filelocation); + if(load_gppc_scenario(in) != std::errc{}) + { + WARTHOG_GERROR_FMT( + "Failed to load scenario file \"{}\"", sfile_.string()); + throw std::runtime_error("scenario_manager failed to load scenario"); + } +} - if(!infile.good()) +void +scenario_manager::load_scenario( + std::istream& file, std::filesystem::path&& scenfile) +{ + sfile_ = std::move(scenfile); + load_gppc_scenario(file); + if(load_gppc_scenario(file) != std::errc{}) { - std::cerr << "err; scenario_manager::load_scenario " - << "Invalid scenario file: " << filelocation << std::endl; - infile.close(); - exit(1); + WARTHOG_GERROR_FMT( + "Failed to load scenario file \"{}\"", std::string(sfile_)); + throw std::runtime_error("scenario_manager failed to load scenario"); } +} - sfile_ = filelocation; +// V1.0 is the version officially supported by HOG +std::errc +scenario_manager::load_gppc_scenario(std::istream& scenfile) +{ + clear(); - char buf[1024]; - infile.getline(buf, 1024); - if(strstr(buf, "version 1")) + io::scenario_serialize si; + si.set_scenario_filename(std::filesystem::path(sfile_)); + si.set_force_int(true); + + // open stream and read header + if(auto ec = si.open_read(&scenfile); ec != std::errc{}) + { + WARTHOG_GERROR("scenario_manager failed to open for read"); + return ec; + } + if(auto ec = si.read_version(); ec != std::errc{}) + { + WARTHOG_GERROR_FMT( + "scenario_manager failed to read version on line: {}", + si.get_line_num()); + return std::errc::io_error; + } + auto version = si.get_version(); + if(version == io::scenario_version::VERSION_1) + { + return load_gppc_scenario_body_v1(si); + } + else if(version == io::scenario_version::VERSION_2) { - // GPPC format scenarios - load_gppc_scenario(infile); + return load_gppc_scenario_body_v2(si); } else { - std::cerr << "err; scenario_manager::load_scenario " - << " scenario file not in GPPC format\n"; - infile.close(); - exit(1); + WARTHOG_GERROR("scenario_manager reading unsupported version"); + return std::errc::invalid_argument; } - infile.close(); } -// V1.0 is the version officially supported by HOG -void -scenario_manager::load_gppc_scenario(std::ifstream& infile) +std::errc +scenario_manager::load_gppc_scenario_body_v1(io::scenario_serialize& si) { - uint32_t sizeX = 0, sizeY = 0; - uint32_t bucket; - std::string map; - uint32_t xs, ys, xg, yg; - std::string dist; + if(auto ec = si.read_header(); ec != std::errc{}) + { + WARTHOG_GERROR_FMT( + "scenario_manager failed to read scenario v1 header on line: {}", + si.get_line_num()); + return std::errc::io_error; + } - while(infile >> bucket >> map >> sizeX >> sizeY >> xs >> ys >> xg >> yg - >> dist) + mfile_ = si.get_map_filename(); + experiments_.reserve(1024); + commands_.reserve(1024); + scenario_width_ = si.get_map_width(); + scenario_height_ = si.get_map_height(); + // setup commands header for static scenario + commands_.push_back(scenario_command::make_snapshot(0, 0)); + commands_.push_back(scenario_command::make_patch(0, 0, 0, 0)); + ++patch_count_; + static_scenario_start_ = (int32_t)commands_.size(); + // read queries until done + bool first = true; + io::scenario_instance Q; + std::string_view map_string; + while(true) { - double dbl_dist = strtod(dist.c_str(), 0); - experiments_.push_back( - new experiment(xs, ys, xg, yg, sizeX, sizeY, dbl_dist, map)); + Q.reset(); + auto [con, ec] = si.read_instance_line(Q); + if(ec != std::errc{}) + { + WARTHOG_GERROR_FMT( + "scenario_manager failed to read inst on line: {}", + si.get_line_num()); + return std::errc::io_error; + } + if(con == io::scenario_serialize::VALID) + { + std::string_view current_map(Q.map); + if(current_map.size() > 2048) // limit string size + { + WARTHOG_GERROR_FMT( + "scenario_manager v1 inst line map exceeds 2048 chars on " + "line: {}", + si.get_line_num()); + return std::errc::argument_out_of_domain; + } + if(map_string != current_map) + { + map_string = copy_string(current_map); + } + experiment* ex = std::construct_at( + static_cast(experiments_res_.allocate( + sizeof(experiment), alignof(experiment))), + (uint32_t)Q.start_x, (uint32_t)Q.start_y, (uint32_t)Q.goal_x, + (uint32_t)Q.goal_y, si.get_map_width(), si.get_map_height(), + Q.cost[(int)0], map_string); + experiments_.push_back(ex); + commands_.push_back(scenario_command::make_inst( + Q.bucket, inst_count_++, (uint32_t)(experiments_.size() - 1))); + } + else if(con == io::scenario_serialize::FINAL) { break; } + } + return std::errc{}; +} + +std::errc +scenario_manager::load_gppc_scenario_body_v2(io::scenario_serialize& si) +{ + if(auto ec = si.read_header(); ec != std::errc{}) + { + WARTHOG_GERROR_FMT( + "scenario_manager failed to read scenario v2 header on line: {}", + si.get_line_num()); + return std::errc::io_error; + } + + mfile_ = si.get_map_filename(); + experiments_.reserve(1024); + commands_.reserve(1024); + scenario_width_ = si.get_map_width(); + scenario_height_ = si.get_map_height(); + static_scenario_start_ = -1; // init dynamic scenario + + // set map filename + std::string_view map_string = copy_string(si.get_map_filename().string()); + if(map_string.size() > 2048) // limit string size + { + WARTHOG_GERROR("scenario_manager v2 map exceeds 2048 chars"); + return std::errc::argument_out_of_domain; + } + + // get cost index + int cost_index = si.get_cost_type().size() != 0 ? 0 : -1; + if(!cost_type_.empty()) + { + // user-provided cost index + cost_index = si.find_cost_index(cost_type_); + if(cost_index < 0) + { + WARTHOG_GWARN_FMT( + "scenario_manager v2 failed to find user-provided cost: {}", + cost_type_); + } + } + else + { + // try to use grid if exist + if(int ci = si.find_cost_index(io::cost_type::G_8C_NCC); ci >= 0) + { + cost_index = ci; + } + } + + // read queries until done + io::scenario_instance Q; + io::scenario_patch P; + int last_type = -1; + int last_bucket = -1; + int snapshot_count = -1; + while(true) + { + // try reading a inst line + int con; + std::errc ec; + std::tie(con, ec) = si.read_instance_line(Q); + if(ec != std::errc{}) + { + WARTHOG_GERROR_FMT( + "scenario_manager failed to read command on line: {}", + si.get_line_num()); + return std::errc::io_error; + } + if(con == io::scenario_serialize::VALID) + { + if(last_type == -1) + { + // only used if first command is a inst + commands_.push_back(scenario_command::make_snapshot( + Q.bucket, ++snapshot_count)); + } + last_type = io::scenario_serialize::CMD_INST; + last_bucket = Q.bucket; + + experiment* ex = std::construct_at( + static_cast(experiments_res_.allocate( + sizeof(experiment), alignof(experiment))), + Q.start_x, Q.start_y, Q.goal_x, Q.goal_y, si.get_map_width(), + si.get_map_height(), Q.cost[(int)io::cost_type::G_8C_NCC], + map_string); + experiments_.push_back(ex); + commands_.push_back(scenario_command::make_inst( + Q.bucket, inst_count_++, (uint32_t)(experiments_.size() - 1))); + } + else if(con == io::scenario_serialize::CMD_PATCH) + { + std::tie(con, ec) = si.read_patch_line(P); + if(ec != std::errc{} || con != io::scenario_serialize::VALID) + { + WARTHOG_GERROR_FMT( + "scenario_manager failed to read command on line: {}", + si.get_line_num()); + return std::errc::io_error; + } + if(last_type != io::scenario_serialize::CMD_PATCH + || last_bucket != P.bucket) + { + commands_.push_back(scenario_command::make_snapshot( + P.bucket, ++snapshot_count)); + } + last_type = io::scenario_serialize::CMD_PATCH; + last_bucket = P.bucket; - int32_t precision = 0; - if(dist.find(".") != std::string::npos) + commands_.push_back(scenario_command::make_patch( + P.bucket, P.patch_id, P.loc_x, P.loc_y)); + } + else if(con == io::scenario_serialize::FINAL) { break; } + else if(con == io::scenario_serialize::CMD_UNKNOWN) + { + // ignore + si.skip_commands(); + } + else { - precision = ((int32_t)dist.size() - (int32_t)(dist.find(".") + 1)); + // error, invalid inst + WARTHOG_GERROR_FMT( + "scenario_manager failed to read command on line: {}", + si.get_line_num()); + return std::errc::io_error; } - experiments_.back()->set_precision(precision); } + return std::errc{}; +} + +std::string_view +scenario_manager::copy_string(std::string_view str) +{ + if(str.empty()) return std::string_view(); + char* mapchars + = static_cast(experiments_res_.allocate(str.size() + 1)); + std::memcpy(mapchars, str.data(), str.size()); + mapchars[str.size()] = '\0'; + return std::string_view(mapchars, str.size()); } void @@ -100,23 +315,58 @@ scenario_manager::write_scenario(std::ostream& scenariofile) // scenariofile.close(); } -void -scenario_manager::sort() +/** + * Finds a matching map file to a scenario. + * Take mappath as scenmgr map name. scendir as partent(sfilename), or + * current_working_dir. Returns path in order below: If mappath is absolute + * path: if exists return mappath, else return no path. If scendir/mappath + * exists: return scendir/mappath. If sfilename != '' and replace sfilename ext + * to '.map': if exists return that. If sfilename != '' and remove sfilename + * ext: if new extension is '.map' and exists return that. Return empty path. + */ +std::filesystem::path +find_map_filename( + const scenario_manager& scenmgr, const std::filesystem::path& sfilename) { - for(unsigned int i = 0; i < experiments_.size(); i++) + namespace fs = std::filesystem; + const auto& mapname = scenmgr.get_experiment(0)->map(); + // scen file has a map name designated. + if(!mapname.empty()) { - for(unsigned int j = i; j < experiments_.size(); j++) + auto mappath = fs::path(mapname); + // absolute path, try to use that only. + if(mappath.is_absolute()) { - if(experiments_.at(j)->distance() < experiments_.at(i)->distance()) - { - experiment* tmp = experiments_.at(i); - experiments_.at(i) = experiments_.at(j); - experiments_.at(j) = tmp; - } + if(fs::is_regular_file(mappath)) { return mappath; } + else { return {}; } + } + // path is relative path + auto spath = !sfilename.empty() ? sfilename.parent_path() + : fs::current_path(); + // check relative path from either sfilename directory or + // current_working_directory + if(auto append_path = spath / mapname; + fs::is_regular_file(append_path)) + { + return append_path; } } + // if a scenario filename was presented, try to deduce map from scenario + // filename + if(!sfilename.empty()) + { + // replace extenion with .map + auto mapfile = sfilename; + mapfile.replace_extension(".map"); + if(fs::is_regular_file(mapfile)) return mapfile; + // remove extension and check it is now .map (test for .map.scen) + mapfile.replace_extension(""); + if(mapfile.extension() == ".map" && fs::is_regular_file(mapfile)) + return mapfile; + } + // no clear way to deduce map, return empty path for no success + return {}; } - /** * Finds a matching map file to a scenario. * Take mappath as scenmgr map name. scendir as partent(sfilename), or @@ -128,10 +378,10 @@ scenario_manager::sort() */ std::filesystem::path find_map_filename( - const scenario_manager& scenmgr, std::filesystem::path sfilename) + const std::filesystem::path& mapname, + const std::filesystem::path& sfilename) { - namespace fs = std::filesystem; - const auto& mapname = scenmgr.get_experiment(0)->map(); + namespace fs = std::filesystem; // scen file has a map name designated. if(!mapname.empty()) { @@ -158,10 +408,11 @@ find_map_filename( if(!sfilename.empty()) { // replace extenion with .map - auto mapfile = sfilename.replace_extension(".map"); + auto mapfile = sfilename; + mapfile.replace_extension(".map"); if(fs::is_regular_file(mapfile)) return mapfile; // remove extension and check it is now .map (test for .map.scen) - mapfile = sfilename.replace_extension(""); + mapfile.replace_extension(""); if(mapfile.extension() == ".map" && fs::is_regular_file(mapfile)) return mapfile; } diff --git a/src/util/timer.cpp b/src/util/timer.cpp index 702b7a3..0029b6b 100644 --- a/src/util/timer.cpp +++ b/src/util/timer.cpp @@ -1,6 +1,7 @@ -#include #include +#include + namespace warthog::util {