diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 6ec57cd7..c94f791a 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -110,6 +110,9 @@ protected: /// copy external_state to a child's interface and remember the link in internal_to map void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); + /// lift solution from internal to external level + void liftSolution(SolutionBase& solution, + const InterfaceState *internal_from, const InterfaceState *internal_to); protected: container_type children_; @@ -157,12 +160,7 @@ class SerialContainerPrivate : public ContainerBasePrivate { friend class SerialContainer; public: - SerialContainerPrivate(SerialContainer* me, const std::string &name) - : ContainerBasePrivate(me, name) - {} - - void storeNewSolution(SerialSolution&& s); - const ordered& solutions() const { return solutions_; } + SerialContainerPrivate(SerialContainer* me, const std::string &name); private: void connect(StagePrivate *prev, StagePrivate *next); @@ -179,8 +177,12 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate { public: ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name); +private: /// callback for new externally received states void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); + + // set of all solutions + ordered solutions_; }; PIMPL_FUNCTIONS(ParallelContainerBase) diff --git a/core/src/container.cpp b/core/src/container.cpp index 8a11e608..597cc557 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -106,6 +106,36 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa internal_to_external_.insert(std::make_pair(&internal, external)); } +void ContainerBasePrivate::liftSolution(SolutionBase& solution, + const InterfaceState *internal_from, const InterfaceState *internal_to) +{ + // add solution to existing or new start state + auto it = internal_to_external_.find(internal_from); + if (it != internal_to_external_.end()) { + // connect solution to existing start state + solution.setStartState(*it->second); + } else { + // spawn a new state in previous stage + Interface::iterator external = prevEnds()->add(InterfaceState(*internal_from), NULL, &solution); + internal_to_external_.insert(std::make_pair(internal_from, external)); + } + + // add solution to existing or new end state + it = internal_to_external_.find(internal_to); + if (it != internal_to_external_.end()) { + // connect solution to existing start state + solution.setEndState(*it->second); + } else { + // spawn a new state in next stage + Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); + internal_to_external_.insert(std::make_pair(internal_to, external)); + } + + // perform default stage action on new solution + StagePrivate::newSolution(solution); +} + + ContainerBase::ContainerBase(ContainerBasePrivate *impl) : Stage(impl) { @@ -268,42 +298,10 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) } // store new solutions (in sorted) - for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) - impl->storeNewSolution(std::move(*it)); -} - -void SerialContainerPrivate::storeNewSolution(SerialSolution &&s) -{ - const InterfaceState *internal_from = s.internalStart(); - const InterfaceState *internal_to = s.internalEnd(); - - // create new SerialSolution and get a reference to it - SerialSolution& solution = *solutions_.insert(std::move(s)); - - // add solution to existing or new start state - auto it = internal_to_external_.find(internal_from); - if (it != internal_to_external_.end()) { - // connect solution to existing start state - solution.setStartState(*it->second); - } else { - // spawn a new state in previous stage - Interface::iterator external = prevEnds()->add(InterfaceState(*internal_from), NULL, &solution); - internal_to_external_.insert(std::make_pair(internal_from, external)); + for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) { + auto inserted = impl->solutions_.insert(std::move(*it)); + impl->liftSolution(*inserted, inserted->internalStart(), inserted->internalEnd()); } - - // add solution to existing or new end state - it = internal_to_external_.find(internal_to); - if (it != internal_to_external_.end()) { - // connect solution to existing start state - solution.setEndState(*it->second); - } else { - // spawn a new state in next stage - Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); - internal_to_external_.insert(std::make_pair(internal_to, external)); - } - - // perform default stage action on new solution - newSolution(solution); } @@ -325,6 +323,10 @@ void SerialContainer::reset() ContainerBase::reset(); } +SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name) + : ContainerBasePrivate(me, name) +{} + void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) { prev->setNextStarts(next->starts()); next->setPrevEnds(prev->ends()); @@ -410,7 +412,7 @@ size_t SerialContainer::numSolutions() const void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &processor) const { - for(const SolutionBase& s : pimpl()->solutions()) + for(const SolutionBase& s : pimpl()->solutions_) if (!processor(s)) break; } @@ -525,14 +527,9 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc void ParallelContainerBase::onNewSolution(const SolutionBase &s) { - // update state priorities - InterfaceState::Priority prio(1, s.cost()); - InterfaceState* start = const_cast(s.start()); - start->owner()->updatePriority(*start, prio); - InterfaceState* end = const_cast(s.end()); - end->owner()->updatePriority(*end, prio); - - pimpl()->newSolution(s); + auto impl = pimpl(); + auto it = impl->solutions_.insert(WrappedSolution(impl, &s)); + impl->liftSolution(*it, s.start(), s.end()); } @@ -591,6 +588,7 @@ Stage* WrapperBase::wrapped() void WrapperBase::onNewSolution(const SolutionBase &s) { + // TODO replace with liftSolution() // update state priorities InterfaceState::Priority prio(1, s.cost()); InterfaceState* start = const_cast(s.start());