ContainerBasePrivate::liftSolution()

This commit is contained in:
Robert Haschke 2018-01-31 21:32:42 +01:00
parent 919be24d16
commit 1ab2d596fb
2 changed files with 50 additions and 50 deletions

View File

@ -110,6 +110,9 @@ protected:
/// copy external_state to a child's interface and remember the link in internal_to map /// 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); 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: protected:
container_type children_; container_type children_;
@ -157,12 +160,7 @@ class SerialContainerPrivate : public ContainerBasePrivate {
friend class SerialContainer; friend class SerialContainer;
public: public:
SerialContainerPrivate(SerialContainer* me, const std::string &name) SerialContainerPrivate(SerialContainer* me, const std::string &name);
: ContainerBasePrivate(me, name)
{}
void storeNewSolution(SerialSolution&& s);
const ordered<SerialSolution>& solutions() const { return solutions_; }
private: private:
void connect(StagePrivate *prev, StagePrivate *next); void connect(StagePrivate *prev, StagePrivate *next);
@ -179,8 +177,12 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate {
public: public:
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name); ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name);
private:
/// callback for new externally received states /// callback for new externally received states
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
// set of all solutions
ordered<WrappedSolution> solutions_;
}; };
PIMPL_FUNCTIONS(ParallelContainerBase) PIMPL_FUNCTIONS(ParallelContainerBase)

View File

@ -106,6 +106,36 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
internal_to_external_.insert(std::make_pair(&internal, external)); 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) ContainerBase::ContainerBase(ContainerBasePrivate *impl)
: Stage(impl) : Stage(impl)
{ {
@ -268,42 +298,10 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
} }
// store new solutions (in sorted) // store new solutions (in sorted)
for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) {
impl->storeNewSolution(std::move(*it)); auto inserted = impl->solutions_.insert(std::move(*it));
} impl->liftSolution(*inserted, inserted->internalStart(), inserted->internalEnd());
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));
} }
// 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(); ContainerBase::reset();
} }
SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name)
: ContainerBasePrivate(me, name)
{}
void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) { void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) {
prev->setNextStarts(next->starts()); prev->setNextStarts(next->starts());
next->setPrevEnds(prev->ends()); next->setPrevEnds(prev->ends());
@ -410,7 +412,7 @@ size_t SerialContainer::numSolutions() const
void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &processor) const void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &processor) const
{ {
for(const SolutionBase& s : pimpl()->solutions()) for(const SolutionBase& s : pimpl()->solutions_)
if (!processor(s)) if (!processor(s))
break; break;
} }
@ -525,14 +527,9 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc
void ParallelContainerBase::onNewSolution(const SolutionBase &s) void ParallelContainerBase::onNewSolution(const SolutionBase &s)
{ {
// update state priorities auto impl = pimpl();
InterfaceState::Priority prio(1, s.cost()); auto it = impl->solutions_.insert(WrappedSolution(impl, &s));
InterfaceState* start = const_cast<InterfaceState*>(s.start()); impl->liftSolution(*it, s.start(), s.end());
start->owner()->updatePriority(*start, prio);
InterfaceState* end = const_cast<InterfaceState*>(s.end());
end->owner()->updatePriority(*end, prio);
pimpl()->newSolution(s);
} }
@ -591,6 +588,7 @@ Stage* WrapperBase::wrapped()
void WrapperBase::onNewSolution(const SolutionBase &s) void WrapperBase::onNewSolution(const SolutionBase &s)
{ {
// TODO replace with liftSolution()
// update state priorities // update state priorities
InterfaceState::Priority prio(1, s.cost()); InterfaceState::Priority prio(1, s.cost());
InterfaceState* start = const_cast<InterfaceState*>(s.start()); InterfaceState* start = const_cast<InterfaceState*>(s.start());