From 7609f0b1e2b48db21d69041bd5fd257ed9ef85c4 Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 29 Jun 2020 23:50:01 +0200 Subject: [PATCH] Solution: export public creator It's a public interface, returning private pointer is not neat. --- core/include/moveit/task_constructor/container_p.h | 8 ++++---- core/include/moveit/task_constructor/storage.h | 10 +++++----- core/src/container.cpp | 10 +++++----- core/src/stage.cpp | 2 +- core/src/stages/connect.cpp | 2 +- core/src/storage.cpp | 4 ++-- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 61eae9a1..ebdc119f 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -195,11 +195,11 @@ PIMPL_FUNCTIONS(SerialContainer) class WrappedSolution : public SolutionBase { public: - explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost, std::string comment) + explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped, double cost, std::string comment) : SolutionBase(creator, cost, std::move(comment)), wrapped_(wrapped) {} - explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost) + explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped, double cost) : SolutionBase(creator, cost), wrapped_(wrapped) {} - explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped) + explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped) : WrappedSolution(creator, wrapped, wrapped->cost()) {} void fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const override; @@ -244,7 +244,7 @@ class MergerPrivate : public ParallelContainerBasePrivate moveit::core::JointModelGroupPtr jmg_merged_; using ChildSolutionList = std::vector; - using ChildSolutionMap = std::map; + using ChildSolutionMap = std::map; // map from external source state (iterator) to all corresponding children's solutions std::map source_state_to_solutions_; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 2db0036d..c57cd426 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -220,8 +220,8 @@ public: const_cast(state).addIncoming(this); } - inline const StagePrivate* creator() const { return creator_; } - void setCreator(StagePrivate* creator); + inline const Stage* creator() const { return creator_; } + void setCreator(Stage* creator); inline double cost() const { return cost_; } void setCost(double cost); @@ -243,12 +243,12 @@ public: bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; } protected: - SolutionBase(StagePrivate* creator = nullptr, double cost = 0.0, std::string comment = "") + SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "") : creator_(creator), cost_(cost), comment_(std::move(comment)) {} private: // back-pointer to creating stage, allows to access sub-solutions - StagePrivate* creator_; + Stage* creator_; // associated cost double cost_; // comment for this solution, e.g. explanation of failure @@ -293,7 +293,7 @@ public: using container_type = std::vector; explicit SolutionSequence() : SolutionBase() {} - SolutionSequence(container_type&& subsolutions, double cost = 0.0, StagePrivate* creator = nullptr) + SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr) : SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {} void push_back(const SolutionBase& solution); diff --git a/core/src/container.cpp b/core/src/container.cpp index 664bb168..7655f3a4 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -292,7 +292,7 @@ struct SolutionCollector // Traced path should not extend past container boundaries, i.e. trace.size() <= max_depth // However, as the Merging-Connect's solution may be composed of several subsolutions, we need to disregard those size_t len = trace.size(); - const StagePrivate* prev_creator = nullptr; + const Stage* prev_creator = nullptr; for (const auto& s : trace) { if (s->creator() == prev_creator) --len; @@ -328,13 +328,13 @@ void updateStateCosts(const SolutionSequence::container_type& partial_solution_p void SerialContainer::onNewSolution(const SolutionBase& current) { auto impl = pimpl(); - const StagePrivate* creator = current.creator(); + const Stage* creator = current.creator(); auto& children = impl->children(); // find number of stages before and after creator stage size_t num_before = 0, num_after = 0; for (auto it = children.begin(), end = children.end(); it != end; ++it, ++num_before) - if ((*it)->pimpl() == creator) + if (&(**it) == creator) break; assert(num_before < children.size()); // creator should be one of our children num_after = children.size() - 1 - num_before; @@ -370,7 +370,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // insert outgoing solutions in normal order solution.insert(solution.end(), out.first.begin(), out.first.end()); // store solution in sorted list - sorted.insert(std::make_shared(std::move(solution), prio.cost(), impl)); + sorted.insert(std::make_shared(std::move(solution), prio.cost(), this)); } else if (prio.depth() > 1) { // update state priorities along the whole partial solution path updateStateCosts(in.first, prio); @@ -650,7 +650,7 @@ ParallelContainerBase::ParallelContainerBase(const std::string& name) void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) { auto impl = pimpl(); - impl->liftSolution(std::make_shared(impl, &solution, cost, std::move(comment)), solution.start(), + impl->liftSolution(std::make_shared(this, &solution, cost, std::move(comment)), solution.start(), solution.end()); } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 3c4c9415..38761006 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -125,7 +125,7 @@ void StagePrivate::validateConnectivity() const { } bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { - solution->setCreator(this); + solution->setCreator(me()); if (introspection_) introspection_->registerSolution(*solution); diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index ded74dde..0d07f52d 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -195,7 +195,7 @@ Connect::makeSequential(const std::vectorsetCreator(pimpl_); + inserted->setCreator(this); // push back solution pointer sub_solutions.push_back(&*inserted); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 245992c8..642063bc 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -136,7 +136,7 @@ void Interface::updatePriority(InterfaceState* state, const InterfaceState::Prio } } -void SolutionBase::setCreator(StagePrivate* creator) { +void SolutionBase::setCreator(Stage* creator) { assert(creator_ == nullptr || creator_ == creator); // creator must only set once creator_ = creator; } @@ -150,7 +150,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, In info.cost = this->cost(); info.comment = this->comment(); const Introspection* ci = introspection; - info.stage_id = ci ? ci->stageId(this->creator()->me()) : 0; + info.stage_id = ci ? ci->stageId(this->creator()) : 0; const auto& markers = this->markers(); info.markers.resize(markers.size());