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

View File

@ -75,7 +75,7 @@ MOVEIT_CLASS_FORWARD(Introspection)
*/ */
class InterfaceState { class InterfaceState {
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase 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: public:
/** InterfaceStates are ordered according to two values: /** InterfaceStates are ordered according to two values:
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path. * Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
@ -120,6 +120,7 @@ public:
return this->priority_ < other.priority_; return this->priority_ < other.priority_;
} }
inline const Priority& priority() const { return priority_; } inline const Priority& priority() const { return priority_; }
Interface* owner() const { return owner_; }
private: private:
// these methods should be only called by SolutionBase::set[Start|End]State() // these methods should be only called by SolutionBase::set[Start|End]State()
@ -131,7 +132,10 @@ private:
PropertyMap properties_; PropertyMap properties_;
Solutions incoming_trajectories_; Solutions incoming_trajectories_;
Solutions outgoing_trajectories_; Solutions outgoing_trajectories_;
// members needed for priority scheduling in Interface list
Priority priority_; Priority priority_;
Interface* owner_ = nullptr; // allow update of priority
}; };
@ -141,12 +145,15 @@ public:
typedef std::function<void(iterator it, bool updated)> NotifyFunction; typedef std::function<void(iterator it, bool updated)> NotifyFunction;
Interface(const NotifyFunction &notify = 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); 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); 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: private:
const NotifyFunction notify_; const NotifyFunction notify_;
}; };
@ -164,13 +171,13 @@ public:
inline const InterfaceState* end() const { return end_; } inline const InterfaceState* end() const { return end_; }
inline void setStartState(const InterfaceState& state){ inline void setStartState(const InterfaceState& state){
assert(start_ == NULL); assert(start_ == NULL); // only allow setting once (by Stage)
start_ = &state; start_ = &state;
const_cast<InterfaceState&>(state).addOutgoing(this); const_cast<InterfaceState&>(state).addOutgoing(this);
} }
inline void setEndState(const InterfaceState& state){ inline void setEndState(const InterfaceState& state){
assert(end_ == NULL); assert(end_ == NULL); // only allow setting once (by Stage)
end_ = &state; end_ = &state;
const_cast<InterfaceState&>(state).addIncoming(this); 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()); 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); SolutionCollector incoming(num_before);
traverse<BACKWARD>(current, std::ref(incoming), trace); traverse<BACKWARD>(current, std::ref(incoming), trace);
if (incoming.solutions.empty())
return; // no connection to front()
// find all outgoing solution pathes starting at current solution
// find all outgoing trajectories connected to current solution
SolutionCollector outgoing(num_after); SolutionCollector outgoing(num_after);
traverse<FORWARD>(current, std::ref(outgoing), trace); 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; ordered<SerialSolution> sorted;
SerialContainer::solution_container solution; SerialContainer::solution_container solution;
solution.reserve(children.size()); solution.reserve(children.size());
for (auto& in : incoming.solutions) { for (auto& in : incoming.solutions) {
for (auto& out : outgoing.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()); assert(solution.empty());
// insert incoming solutions in reverse order // insert incoming solutions in reverse order
solution.insert(solution.end(), in.first.rbegin(), in.first.rend()); solution.insert(solution.end(), in.first.rbegin(), in.first.rend());
@ -261,12 +260,19 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
solution.push_back(&current); solution.push_back(&current);
// insert outgoing solutions in normal order // insert outgoing solutions in normal order
solution.insert(solution.end(), out.first.begin(), out.first.end()); solution.insert(solution.end(), out.first.begin(), out.first.end());
// store solution in sorted list
sorted.insert(SerialSolution(impl, std::move(solution), in.second + current.cost() + out.second)); 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) for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it)
impl->storeNewSolution(std::move(*it)); impl->storeNewSolution(std::move(*it));
} }
@ -507,6 +513,18 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc
throw errors; 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) WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
: ContainerBasePrivate(me, name) : ContainerBasePrivate(me, name)
@ -569,6 +587,18 @@ Stage* WrapperBase::wrapped()
return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get(); 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) WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name)
: WrapperBasePrivate(me, name) : WrapperBasePrivate(me, name)

View File

@ -69,6 +69,9 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin
// move state to a list node // move state to a list node
std::list<InterfaceState> container; std::list<InterfaceState> container;
auto it = container.insert(container.end(), std::move(state)); 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 // 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 assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set
if (incoming) { if (incoming) {
@ -88,10 +91,20 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin
Interface::iterator Interface::clone(const InterfaceState &state) Interface::iterator Interface::clone(const InterfaceState &state)
{ {
iterator it = insert(InterfaceState(state)); iterator it = insert(InterfaceState(state));
it->owner_ = this;
if (notify_) notify_(it, false); if (notify_) notify_(it, false);
return it; 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) void SolutionBase::setCreator(StagePrivate *creator)
{ {

View File

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