From ab83dfc898fb1b9e99c79ca4aa0a7940bbdca7f9 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 10 Jan 2025 15:22:17 +1100 Subject: [PATCH] Feature/gridmap utility apply (#7) * update gridmap to new core utility functions --- apps/jps.cpp | 33 ++++++++++++++----- extern/warthog-core | 2 +- include/jps/jump/online_jump_point_locator.h | 4 +-- include/jps/jump/online_jump_point_locator2.h | 4 +-- src/jump/offline_jump_point_locator.cpp | 2 +- src/jump/offline_jump_point_locator2.cpp | 2 +- src/jump/online_jump_point_locator.cpp | 5 +-- src/jump/online_jump_point_locator2.cpp | 5 +-- src/search/jps.cpp | 2 +- 9 files changed, 38 insertions(+), 21 deletions(-) diff --git a/apps/jps.cpp b/apps/jps.cpp index ffe61d2..09da234 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -111,21 +111,24 @@ check_optimality( } template -void +int run_experiments( Search& algo, std::string alg_name, warthog::util::scenario_manager& scenmgr, bool verbose, bool checkopt, std::ostream& out) { - std::cout << "id\talg\texpanded\tgenerated\treopen\tsurplus\theapops" - << "\tnanos\tplen\tpcost\tscost\tmap\n"; + auto* expander = algo.get_expander(); + if(expander == nullptr) return 1; + out << "id\talg\texpanded\tgenerated\treopen\tsurplus\theapops" + << "\tnanos\tplen\tpcost\tscost\tmap\n"; for(unsigned int i = 0; i < scenmgr.num_experiments(); i++) { warthog::util::experiment* exp = scenmgr.get_experiment(i); - warthog::pack_id startid{ - exp->starty() * exp->mapwidth() + exp->startx()}; - warthog::pack_id goalid{exp->goaly() * exp->mapwidth() + exp->goalx()}; + warthog::pack_id startid + = expander->get_pack(exp->startx(), exp->starty()); + warthog::pack_id goalid + = expander->get_pack(exp->goalx(), exp->goaly()); warthog::search::problem_instance pi(startid, goalid, verbose); warthog::search::search_parameters par; warthog::search::solution sol; @@ -141,12 +144,17 @@ run_experiments( << exp->distance() << "\t" << scenmgr.last_file_loaded() << std::endl; - if(checkopt) { check_optimality(sol, exp); } + if(checkopt) + { + if(!check_optimality(sol, exp)) return 4; + } } + + return 0; } template -void +int run_jps( warthog::util::scenario_manager& scenmgr, std::string mapname, std::string alg_name) @@ -158,8 +166,15 @@ run_jps( warthog::search::unidirectional_search jps(&heuristic, &expander, &open); - run_experiments(jps, alg_name, scenmgr, verbose, checkopt, std::cout); + int ret = run_experiments( + jps, alg_name, scenmgr, verbose, checkopt, std::cout); + if(ret != 0) + { + std::cerr << "run_experiments error code " << ret << std::endl; + return ret; + } std::cerr << "done. total memory: " << jps.mem() + scenmgr.mem() << "\n"; + return 0; } } // namespace diff --git a/extern/warthog-core b/extern/warthog-core index 6586def..3574fb1 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 6586def3f6b205dab1e396505a9f3012c99e874d +Subproject commit 3574fb1cf7bab2ae3a9b5b322bd66bd1fed84cc4 diff --git a/include/jps/jump/online_jump_point_locator.h b/include/jps/jump/online_jump_point_locator.h index b2bdace..85fde3f 100644 --- a/include/jps/jump/online_jump_point_locator.h +++ b/include/jps/jump/online_jump_point_locator.h @@ -101,7 +101,7 @@ class online_jump_point_locator map_->to_unpadded_xy(mapid, x, y); ry = x; rx = map_->header_height() - y - 1; - return jps_id(rmap_->to_padded_id(rx, ry)); + return jps_id(rmap_->to_padded_id_from_unpadded(rx, ry)); } inline jps_id @@ -114,7 +114,7 @@ class online_jump_point_locator rmap_->to_unpadded_xy(rmapid, rx, ry); x = ry; y = rmap_->header_width() - rx - 1; - return jps_id(map_->to_padded_id(x, y)); + return jps_id(map_->to_padded_id_from_unpadded(x, y)); } warthog::domain::gridmap* diff --git a/include/jps/jump/online_jump_point_locator2.h b/include/jps/jump/online_jump_point_locator2.h index 0e6739f..ed2aff2 100644 --- a/include/jps/jump/online_jump_point_locator2.h +++ b/include/jps/jump/online_jump_point_locator2.h @@ -111,7 +111,7 @@ class online_jump_point_locator2 map_->to_unpadded_xy(mapid, x, y); ry = x; rx = map_->header_height() - y - 1; - return jps_id(rmap_->to_padded_id(rx, ry)); + return jps_id(rmap_->to_padded_id_from_unpadded(rx, ry)); } // convert rmap indexes to map indexes @@ -125,7 +125,7 @@ class online_jump_point_locator2 rmap_->to_unpadded_xy(rmapid, rx, ry); x = ry; y = rmap_->header_width() - rx - 1; - return jps_id(map_->to_padded_id(x, y)); + return jps_id(map_->to_padded_id_from_unpadded(x, y)); } warthog::domain::gridmap* diff --git a/src/jump/offline_jump_point_locator.cpp b/src/jump/offline_jump_point_locator.cpp index e03a884..a3b372b 100644 --- a/src/jump/offline_jump_point_locator.cpp +++ b/src/jump/offline_jump_point_locator.cpp @@ -36,7 +36,7 @@ offline_jump_point_locator::preproc() { for(uint32_t x = 0; x < map_->header_width(); x++) { - jps_id mapid = jps_id{map_->to_padded_id(x, y)}; + jps_id mapid = jps_id{map_->to_padded_id_from_unpadded(x, y)}; // std::cout << mapid << " "; for(uint32_t i = 0; i < 8; i++) { diff --git a/src/jump/offline_jump_point_locator2.cpp b/src/jump/offline_jump_point_locator2.cpp index f3f1c3e..f96b372 100644 --- a/src/jump/offline_jump_point_locator2.cpp +++ b/src/jump/offline_jump_point_locator2.cpp @@ -47,7 +47,7 @@ offline_jump_point_locator2::preproc() { for(uint32_t x = 0; x < map_->header_width(); x++) { - jps_id mapid = jps_id{map_->to_padded_id(x, y)}; + jps_id mapid = jps_id{map_->to_padded_id_from_unpadded(x, y)}; // std::cout << mapid << " "; for(uint32_t i = 0; i < 8; i++) { diff --git a/src/jump/online_jump_point_locator.cpp b/src/jump/online_jump_point_locator.cpp index 4951f65..a6a7d82 100644 --- a/src/jump/online_jump_point_locator.cpp +++ b/src/jump/online_jump_point_locator.cpp @@ -36,10 +36,11 @@ online_jump_point_locator::create_rmap() { for(uint32_t y = 0; y < maph; y++) { - uint32_t label = map_->get_label(map_->to_padded_id(x, y)); + uint32_t label + = map_->get_label(map_->to_padded_id_from_unpadded(x, y)); uint32_t rx = ((rmapw - 1) - y); uint32_t ry = x; - rmap->set_label(rmap->to_padded_id(rx, ry), label); + rmap->set_label(rmap->to_padded_id_from_unpadded(rx, ry), label); } } return rmap; diff --git a/src/jump/online_jump_point_locator2.cpp b/src/jump/online_jump_point_locator2.cpp index f7563c0..830a4f4 100644 --- a/src/jump/online_jump_point_locator2.cpp +++ b/src/jump/online_jump_point_locator2.cpp @@ -38,10 +38,11 @@ online_jump_point_locator2::create_rmap() { for(uint32_t y = 0; y < maph; y++) { - uint32_t label = map_->get_label(map_->to_padded_id(x, y)); + uint32_t label + = map_->get_label(map_->to_padded_id_from_unpadded(x, y)); uint32_t rx = ((rmapw - 1) - y); uint32_t ry = x; - rmap->set_label(rmap->to_padded_id(rx, ry), label); + rmap->set_label(rmap->to_padded_id_from_unpadded(rx, ry), label); } } return rmap; diff --git a/src/search/jps.cpp b/src/search/jps.cpp index 6948dd4..ea16e2f 100644 --- a/src/search/jps.cpp +++ b/src/search/jps.cpp @@ -255,7 +255,7 @@ create_corner_map(warthog::domain::gridmap* gm) { for(uint32_t x = 0; x < mapwidth; x++) { - jps_id from_id = jps_id(gm->to_padded_id(x, y)); + jps_id from_id = jps_id(gm->to_padded_id_from_unpadded(x, y)); if(!gm->get_label(from_id)) { continue; } jps_id w_id = jps_id(from_id.id - 1);