update state priorities in a container-specific fashion

Reorder interface list on priority updates.
This requires the InterfaceState to store a pointer to the owning Interface.
This commit is contained in:
Robert Haschke 2018-01-30 14:58:49 +01:00
parent facd6438c9
commit 5c43d700a8
5 changed files with 78 additions and 24 deletions

View File

@ -138,6 +138,8 @@ public:
protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl);
virtual void onNewSolution(const SolutionBase& s) override;
/// callback for new start states (received externally)
virtual void onNewStartState(Interface::iterator external, bool updated) = 0;
/// callback for new end states (received externally)
@ -192,6 +194,8 @@ public:
}
protected:
virtual void onNewSolution(const SolutionBase& s) = 0;
WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer());
};

View File

@ -75,7 +75,7 @@ MOVEIT_CLASS_FORWARD(Introspection)
*/
class InterfaceState {
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
friend class Interface; // Interface is allowed to change priority
friend class Interface; // allow Interface to set owner_ and priority_
public:
/** InterfaceStates are ordered according to two values:
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
@ -120,6 +120,7 @@ public:
return this->priority_ < other.priority_;
}
inline const Priority& priority() const { return priority_; }
Interface* owner() const { return owner_; }
private:
// these methods should be only called by SolutionBase::set[Start|End]State()
@ -131,7 +132,10 @@ private:
PropertyMap properties_;
Solutions incoming_trajectories_;
Solutions outgoing_trajectories_;
// members needed for priority scheduling in Interface list
Priority priority_;
Interface* owner_ = nullptr; // allow update of priority
};
@ -141,12 +145,15 @@ public:
typedef std::function<void(iterator it, bool updated)> NotifyFunction;
Interface(const NotifyFunction &notify = NotifyFunction());
// add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state
/// add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state
iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing);
// clone an existing InterfaceState, but without its incoming/outgoing connections
/// clone an existing InterfaceState, but without its incoming/outgoing connections
iterator clone(const InterfaceState &state);
/// update state's priority if new priority is smaller and call notify_
void updatePriority(InterfaceState &state, const InterfaceState::Priority &priority);
private:
const NotifyFunction notify_;
};
@ -164,13 +171,13 @@ public:
inline const InterfaceState* end() const { return end_; }
inline void setStartState(const InterfaceState& state){
assert(start_ == NULL);
assert(start_ == NULL); // only allow setting once (by Stage)
start_ = &state;
const_cast<InterfaceState&>(state).addOutgoing(this);
}
inline void setEndState(const InterfaceState& state){
assert(end_ == NULL);
assert(end_ == NULL); // only allow setting once (by Stage)
end_ = &state;
const_cast<InterfaceState&>(state).addIncoming(this);
}

View File

@ -235,25 +235,24 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
SerialContainer::solution_container trace; trace.reserve(children.size());
// find all incoming trajectories connected to current solution
// find all incoming solution pathes ending at current solution
SolutionCollector incoming(num_before);
traverse<BACKWARD>(current, std::ref(incoming), trace);
if (incoming.solutions.empty())
return; // no connection to front()
// find all outgoing trajectories connected to current solution
// find all outgoing solution pathes starting at current solution
SolutionCollector outgoing(num_after);
traverse<FORWARD>(current, std::ref(outgoing), trace);
if (outgoing.solutions.empty())
return; // no connection to back()
// collect (and sort) all solutions for all combinations of incoming + current + outgoing
// collect (and sort) all solutions spanning from start to end of this container
ordered<SerialSolution> sorted;
SerialContainer::solution_container solution;
solution.reserve(children.size());
for (auto& in : incoming.solutions) {
for (auto& out : outgoing.solutions) {
InterfaceState::Priority prio(in.first.size() + 1 + out.first.size(),
in.second + current.cost() + out.second);
// found a complete solution path connecting start to end?
if (prio.depth() == children.size()) {
assert(solution.empty());
// insert incoming solutions in reverse order
solution.insert(solution.end(), in.first.rbegin(), in.first.rend());
@ -261,12 +260,19 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
solution.push_back(&current);
// insert outgoing solutions in normal order
solution.insert(solution.end(), out.first.begin(), out.first.end());
sorted.insert(SerialSolution(impl, std::move(solution), in.second + current.cost() + out.second));
// store solution in sorted list
sorted.insert(SerialSolution(impl, std::move(solution), prio.cost()));
} else {
// update state costs
const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start();
start->owner()->updatePriority(*const_cast<InterfaceState*>(start), prio);
const InterfaceState* end = (out.first.empty() ? current : *out.first.back()).end();
end->owner()->updatePriority(*const_cast<InterfaceState*>(end), prio);
}
}
}
// store new solutions
// store new solutions (in sorted)
for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it)
impl->storeNewSolution(std::move(*it));
}
@ -507,6 +513,18 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc
throw errors;
}
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);
}
WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
: ContainerBasePrivate(me, name)
@ -569,6 +587,18 @@ Stage* WrapperBase::wrapped()
return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get();
}
void WrapperBase::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);
}
WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name)
: WrapperBasePrivate(me, name)

View File

@ -69,6 +69,9 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin
// move state to a list node
std::list<InterfaceState> container;
auto it = container.insert(container.end(), std::move(state));
assert(it->owner_ == nullptr);
it->owner_ = this;
// configure state: inherit priority from other end's state and add current solution's cost
assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set
if (incoming) {
@ -88,10 +91,20 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin
Interface::iterator Interface::clone(const InterfaceState &state)
{
iterator it = insert(InterfaceState(state));
it->owner_ = this;
if (notify_) notify_(it, false);
return it;
}
void Interface::updatePriority(InterfaceState &state, const InterfaceState::Priority& priority)
{
if (priority < state.priority()) {
auto it = std::find_if(begin(), end(), [&state](const InterfaceState& other) { return &state == &other; });
assert(it != end()); // state should be part of this interface
state.priority_ = priority;
if (notify_) notify_(it, true);
}
}
void SolutionBase::setCreator(StagePrivate *creator)
{

View File

@ -240,7 +240,7 @@ void Task::publishAllSolutions(bool wait)
void Task::onNewSolution(const SolutionBase &s)
{
pimpl()->newSolution(s);
WrapperBase::onNewSolution(s);
if (introspection_)
introspection_->publishSolution(s);
}