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
|
/// 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)
|
||||||
|
|
||||||
|
|||||||
@ -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 ¤t)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// 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());
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user