mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
ContainerBasePrivate::liftSolution()
This commit is contained in:
parent
919be24d16
commit
1ab2d596fb
@ -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<SerialSolution>& 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<WrappedSolution> solutions_;
|
||||
};
|
||||
PIMPL_FUNCTIONS(ParallelContainerBase)
|
||||
|
||||
|
||||
@ -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));
|
||||
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());
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
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<InterfaceState*>(s.start());
|
||||
start->owner()->updatePriority(*start, prio);
|
||||
InterfaceState* end = const_cast<InterfaceState*>(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<InterfaceState*>(s.start());
|
||||
|
||||
Loading…
Reference in New Issue
Block a user