From 5c43d700a8dc588ee67eeff149e9e09521b3a21b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 30 Jan 2018 14:58:49 +0100 Subject: [PATCH] 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. --- .../moveit/task_constructor/container.h | 4 ++ .../include/moveit/task_constructor/storage.h | 17 +++-- core/src/container.cpp | 66 ++++++++++++++----- core/src/storage.cpp | 13 ++++ core/src/task.cpp | 2 +- 5 files changed, 78 insertions(+), 24 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index c699f5a5..22178bc3 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -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()); }; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 1ca83a00..9a3a47a3 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -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 NotifyFunction; Interface(const NotifyFunction ¬ify = 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(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(state).addIncoming(this); } diff --git a/core/src/container.cpp b/core/src/container.cpp index c4a4b333..b8346d66 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -235,38 +235,44 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) 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(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(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 sorted; SerialContainer::solution_container solution; solution.reserve(children.size()); for (auto& in : incoming.solutions) { for (auto& out : outgoing.solutions) { - assert(solution.empty()); - // insert incoming solutions in reverse order - solution.insert(solution.end(), in.first.rbegin(), in.first.rend()); - // insert current solution - solution.push_back(¤t); - // 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)); + 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()); + // insert current solution + solution.push_back(¤t); + // insert outgoing solutions in normal order + solution.insert(solution.end(), out.first.begin(), out.first.end()); + // 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(start), prio); + const InterfaceState* end = (out.first.empty() ? current : *out.first.back()).end(); + end->owner()->updatePriority(*const_cast(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(s.start()); + start->owner()->updatePriority(*start, prio); + InterfaceState* end = const_cast(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(s.start()); + start->owner()->updatePriority(*start, prio); + InterfaceState* end = const_cast(s.end()); + end->owner()->updatePriority(*end, prio); + + pimpl()->newSolution(s); +} + WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name) : WrapperBasePrivate(me, name) diff --git a/core/src/storage.cpp b/core/src/storage.cpp index eaa32393..4c0b0775 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -69,6 +69,9 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin // move state to a list node std::list 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) { diff --git a/core/src/task.cpp b/core/src/task.cpp index 1ebb0dad..2d19755e 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -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); }