From 6586def3f6b205dab1e396505a9f3012c99e874d Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 15 Nov 2024 11:23:17 +1100 Subject: [PATCH] Feature/abstract gridmap expansion (#21) * abstract out gridmap based expansion policy expand and node generation * bugfix identity assert, now checks is_none --- include/warthog/constants.h | 2 +- .../warthog/search/gridmap_expansion_policy.h | 26 ++-- .../search/vl_gridmap_expansion_policy.h | 36 +++--- src/search/gridmap_expansion_policy.cpp | 114 ++++++++++-------- src/search/vl_gridmap_expansion_policy.cpp | 112 +++++++++-------- 5 files changed, 171 insertions(+), 119 deletions(-) diff --git a/include/warthog/constants.h b/include/warthog/constants.h index 364371b..10200c2 100644 --- a/include/warthog/constants.h +++ b/include/warthog/constants.h @@ -37,7 +37,7 @@ struct identity_base max()) constexpr identity_base(identity_base alt) : id(static_cast(alt.id)) { - assert(id == alt.id); + assert(id == alt.id || (is_none() && alt.is_none())); } constexpr identity_base(const identity_base&) noexcept = default; constexpr identity_base(identity_base&&) noexcept = default; diff --git a/include/warthog/search/gridmap_expansion_policy.h b/include/warthog/search/gridmap_expansion_policy.h index 2459ea5..7b97da4 100644 --- a/include/warthog/search/gridmap_expansion_policy.h +++ b/include/warthog/search/gridmap_expansion_policy.h @@ -30,14 +30,10 @@ namespace warthog::search { -class gridmap_expansion_policy : public expansion_policy +class gridmap_expansion_policy_base : public expansion_policy { public: - gridmap_expansion_policy(domain::gridmap* map, bool manhattan = false); - virtual ~gridmap_expansion_policy() { } - - void - expand(search_node*, search_problem_instance*) override; + gridmap_expansion_policy_base(domain::gridmap* map); search_problem_instance get_problem_instance(problem_instance* pi) override; @@ -55,6 +51,21 @@ class gridmap_expansion_policy : public expansion_policy void print_node(search_node* n, std::ostream& out) override; + size_t + mem() override; + +protected: + domain::gridmap* map_; +}; + +class gridmap_expansion_policy : public gridmap_expansion_policy_base +{ +public: + gridmap_expansion_policy(domain::gridmap* map, bool manhattan = false); + + void + expand(search_node*, search_problem_instance*) override; + search_node* generate_start_node(search_problem_instance* pi) override; @@ -64,8 +75,7 @@ class gridmap_expansion_policy : public expansion_policy size_t mem() override; -private: - domain::gridmap* map_; +protected: bool manhattan_; }; diff --git a/include/warthog/search/vl_gridmap_expansion_policy.h b/include/warthog/search/vl_gridmap_expansion_policy.h index 4479a42..11fe330 100644 --- a/include/warthog/search/vl_gridmap_expansion_policy.h +++ b/include/warthog/search/vl_gridmap_expansion_policy.h @@ -38,21 +38,12 @@ namespace warthog::search { -class vl_gridmap_expansion_policy : public expansion_policy +class vl_gridmap_expansion_policy_base : public expansion_policy { public: - vl_gridmap_expansion_policy( + vl_gridmap_expansion_policy_base( domain::vl_gridmap* map, util::cost_table& costs); - virtual ~vl_gridmap_expansion_policy(); - - void - expand(search_node*, search_problem_instance*) override; - - size_t - mem() override - { - return expansion_policy::mem() + sizeof(*this) + map_->mem(); - } + virtual ~vl_gridmap_expansion_policy_base(); search_problem_instance get_problem_instance(problem_instance* pi) override; @@ -70,15 +61,30 @@ class vl_gridmap_expansion_policy : public expansion_policy void print_node(search_node* n, std::ostream& out) override; + size_t + mem() override; + +protected: + domain::vl_gridmap* map_; + util::cost_table& costs_; +}; + +class vl_gridmap_expansion_policy : public vl_gridmap_expansion_policy_base +{ +public: + using vl_gridmap_expansion_policy_base::vl_gridmap_expansion_policy_base; + + void + expand(search_node*, search_problem_instance*) override; + search_node* generate_start_node(search_problem_instance* pi) override; search_node* generate_target_node(search_problem_instance* pi) override; -private: - domain::vl_gridmap* map_; - util::cost_table& costs_; + size_t + mem() override; }; } // namespace warthog::search diff --git a/src/search/gridmap_expansion_policy.cpp b/src/search/gridmap_expansion_policy.cpp index 976eb21..9dec9c4 100644 --- a/src/search/gridmap_expansion_policy.cpp +++ b/src/search/gridmap_expansion_policy.cpp @@ -5,10 +5,71 @@ namespace warthog::search { +// +// gridmap_expansion_policy_base +// + +gridmap_expansion_policy_base::gridmap_expansion_policy_base( + domain::gridmap* map) + : expansion_policy(map->height() * map->width()), map_(map) +{ } + +search_problem_instance +gridmap_expansion_policy_base::get_problem_instance(problem_instance* pi) +{ + assert(pi != nullptr); + return convert_problem_instance_to_search(*pi, *map_); +} + +pack_id +gridmap_expansion_policy_base::get_state(pad_id node_id) +{ + return map_->to_unpadded_id(node_id); +} + +pad_id +gridmap_expansion_policy_base::unget_state(pack_id node_id) +{ + return map_->to_padded_id(node_id); +} + +void +gridmap_expansion_policy_base::get_xy(pack_id node_id, int32_t& x, int32_t& y) +{ + uint32_t lx, ly; + map_->to_unpadded_xy(node_id, lx, ly); + x = lx; + y = ly; +} +void +gridmap_expansion_policy_base::get_xy(pad_id node_id, int32_t& x, int32_t& y) +{ + uint32_t lx, ly; + map_->to_unpadded_xy(node_id, lx, ly); + x = lx; + y = ly; +} + +void +gridmap_expansion_policy_base::print_node(search_node* n, std::ostream& out) +{ + uint32_t x, y; + map_->to_unpadded_xy(n->get_id(), x, y); + out << "(" << x << ", " << y << ")..."; + n->print(out); +} + +size_t +gridmap_expansion_policy_base::mem() +{ + return expansion_policy::mem() + + (sizeof(gridmap_expansion_policy_base) - sizeof(expansion_policy)) + + map_->mem(); +} + gridmap_expansion_policy::gridmap_expansion_policy( domain::gridmap* map, bool manhattan) - : expansion_policy(map->height() * map->width()), map_(map), - manhattan_(manhattan) + : gridmap_expansion_policy_base(map), manhattan_(manhattan) { } void @@ -82,51 +143,6 @@ gridmap_expansion_policy::expand( } } -search_problem_instance -gridmap_expansion_policy::get_problem_instance(problem_instance* pi) -{ - assert(pi != nullptr); - return convert_problem_instance_to_search(*pi, *map_); -} - -pack_id -gridmap_expansion_policy::get_state(pad_id node_id) -{ - return map_->to_unpadded_id(node_id); -} - -pad_id -gridmap_expansion_policy::unget_state(pack_id node_id) -{ - return map_->to_padded_id(node_id); -} - -void -gridmap_expansion_policy::get_xy(pack_id node_id, int32_t& x, int32_t& y) -{ - uint32_t lx, ly; - map_->to_unpadded_xy(node_id, lx, ly); - x = lx; - y = ly; -} -void -gridmap_expansion_policy::get_xy(pad_id node_id, int32_t& x, int32_t& y) -{ - uint32_t lx, ly; - map_->to_unpadded_xy(node_id, lx, ly); - x = lx; - y = ly; -} - -void -gridmap_expansion_policy::print_node(search_node* n, std::ostream& out) -{ - uint32_t x, y; - map_->to_unpadded_xy(n->get_id(), x, y); - out << "(" << x << ", " << y << ")..."; - n->print(out); -} - search_node* gridmap_expansion_policy::generate_start_node(search_problem_instance* pi) { @@ -148,7 +164,9 @@ gridmap_expansion_policy::generate_target_node(search_problem_instance* pi) size_t gridmap_expansion_policy::mem() { - return expansion_policy::mem() + sizeof(*this) + map_->mem(); + return gridmap_expansion_policy_base::mem() + + (sizeof(gridmap_expansion_policy) + - sizeof(gridmap_expansion_policy_base)); } } // warthog::expansion_policy diff --git a/src/search/vl_gridmap_expansion_policy.cpp b/src/search/vl_gridmap_expansion_policy.cpp index b878f02..8e666a3 100644 --- a/src/search/vl_gridmap_expansion_policy.cpp +++ b/src/search/vl_gridmap_expansion_policy.cpp @@ -5,12 +5,67 @@ namespace warthog::search { -vl_gridmap_expansion_policy::vl_gridmap_expansion_policy( +vl_gridmap_expansion_policy_base::vl_gridmap_expansion_policy_base( domain::vl_gridmap* map, util::cost_table& costs) : expansion_policy(map->height() * map->width()), map_(map), costs_(costs) { } -vl_gridmap_expansion_policy::~vl_gridmap_expansion_policy() { } +vl_gridmap_expansion_policy_base::~vl_gridmap_expansion_policy_base() { } + +size_t +vl_gridmap_expansion_policy_base::mem() +{ + return expansion_policy::mem() + + (sizeof(vl_gridmap_expansion_policy_base) - sizeof(expansion_policy)) + + map_->mem(); +} + +search_problem_instance +vl_gridmap_expansion_policy_base::get_problem_instance(problem_instance* pi) +{ + assert(pi != nullptr); + return convert_problem_instance_to_search(*pi, *map_); +} + +pack_id +vl_gridmap_expansion_policy_base::get_state(pad_id node_id) +{ + return map_->to_unpadded_id(node_id); +} + +pad_id +vl_gridmap_expansion_policy_base::unget_state(pack_id node_id) +{ + return map_->to_padded_id(node_id); +} + +void +vl_gridmap_expansion_policy_base::get_xy( + pack_id node_id, int32_t& x, int32_t& y) +{ + uint32_t lx, ly; + map_->to_unpadded_xy(node_id, lx, ly); + x = lx; + y = ly; +} +void +vl_gridmap_expansion_policy_base::get_xy( + pad_id node_id, int32_t& x, int32_t& y) +{ + uint32_t lx, ly; + map_->to_unpadded_xy(node_id, lx, ly); + x = lx; + y = ly; +} + +void +vl_gridmap_expansion_policy_base::print_node(search_node* n, std::ostream& out) +{ + uint32_t x, y; + map_->to_unpadded_xy(n->get_id(), x, y); + out << "(" << x << ", " << y << ")..."; + n->print(out); +} void vl_gridmap_expansion_policy::expand( @@ -106,51 +161,6 @@ vl_gridmap_expansion_policy::expand( } } -search_problem_instance -vl_gridmap_expansion_policy::get_problem_instance(problem_instance* pi) -{ - assert(pi != nullptr); - return convert_problem_instance_to_search(*pi, *map_); -} - -pack_id -vl_gridmap_expansion_policy::get_state(pad_id node_id) -{ - return map_->to_unpadded_id(node_id); -} - -pad_id -vl_gridmap_expansion_policy::unget_state(pack_id node_id) -{ - return map_->to_padded_id(node_id); -} - -void -vl_gridmap_expansion_policy::get_xy(pack_id node_id, int32_t& x, int32_t& y) -{ - uint32_t lx, ly; - map_->to_unpadded_xy(node_id, lx, ly); - x = lx; - y = ly; -} -void -vl_gridmap_expansion_policy::get_xy(pad_id node_id, int32_t& x, int32_t& y) -{ - uint32_t lx, ly; - map_->to_unpadded_xy(node_id, lx, ly); - x = lx; - y = ly; -} - -void -vl_gridmap_expansion_policy::print_node(search_node* n, std::ostream& out) -{ - uint32_t x, y; - map_->to_unpadded_xy(n->get_id(), x, y); - out << "(" << x << ", " << y << ")..."; - n->print(out); -} - search_node* vl_gridmap_expansion_policy::generate_start_node(search_problem_instance* pi) { @@ -167,4 +177,12 @@ vl_gridmap_expansion_policy::generate_target_node(search_problem_instance* pi) return generate(pi->target_); } +size_t +vl_gridmap_expansion_policy::mem() +{ + return vl_gridmap_expansion_policy_base::mem() + + (sizeof(vl_gridmap_expansion_policy) + - sizeof(vl_gridmap_expansion_policy_base)); +} + } // namespace warthog::search