From facd6438c9b8351d44c7d8fd1493bed3fcb4b431 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 27 Jan 2018 10:26:03 +0100 Subject: [PATCH] Interface: order states by priority Priority is (depth of solutions, accumulated cost along trace). --- .../moveit/task_constructor/container.h | 4 +- .../moveit/task_constructor/container_p.h | 4 +- .../moveit/task_constructor/cost_queue.h | 30 +++- core/include/moveit/task_constructor/stage.h | 4 + .../include/moveit/task_constructor/stage_p.h | 23 +-- .../include/moveit/task_constructor/storage.h | 73 +++++----- core/src/container.cpp | 39 +++-- core/src/stage.cpp | 137 +++++++++--------- core/src/storage.cpp | 32 ++-- 9 files changed, 194 insertions(+), 152 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 7191dd92..c699f5a5 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -139,9 +139,9 @@ protected: ParallelContainerBase(ParallelContainerBasePrivate* impl); /// callback for new start states (received externally) - virtual void onNewStartState(const InterfaceState &external) = 0; + virtual void onNewStartState(Interface::iterator external, bool updated) = 0; /// callback for new end states (received externally) - virtual void onNewEndState(const InterfaceState &external) = 0; + virtual void onNewEndState(Interface::iterator external, bool updated) = 0; }; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 6c4aea7e..66fadb12 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -98,13 +98,13 @@ protected: {} /// copy external_state to a child's interface and remember the link in internal_to map - void copyState(InterfaceState &external, Stage &child, bool to_start); + void copyState(Interface::iterator external, Stage &child, bool to_start, bool updated); protected: container_type children_; // map start/end states of children (internal) to corresponding states in our external interfaces - std::map internal_to_external_; + std::map internal_to_external_; }; PIMPL_FUNCTIONS(ContainerBase) diff --git a/core/include/moveit/task_constructor/cost_queue.h b/core/include/moveit/task_constructor/cost_queue.h index c17c3e4b..98156294 100644 --- a/core/include/moveit/task_constructor/cost_queue.h +++ b/core/include/moveit/task_constructor/cost_queue.h @@ -50,6 +50,8 @@ public: reference top() { return c.front(); } const_reference top() const { return c.front(); } + reference pop() { reference result = top(); c.pop_front(); return result; } + reference front() { return c.front(); } const_reference front() const { return c.front(); } reference back() { return c.back(); } @@ -68,6 +70,9 @@ public: const_reverse_iterator crbegin() const { return c.rbegin(); } const_reverse_iterator crend() const { return c.rend(); } + /// explicitly sort container, useful if many items have changed their value + void sort() { c.sort(comp); } + iterator insert(const value_type& item) { iterator at = std::upper_bound(c.begin(), c.end(), item, comp); return c.insert(at, item); @@ -79,7 +84,7 @@ public: iterator erase(const_iterator pos) { return c.erase(pos); } - /// update sort position of iterator after changes + /// update sort position of a single item after changes iterator update(iterator &it) { container_type temp; temp.splice(temp.end(), c, it); // move it from c to temp @@ -87,12 +92,26 @@ public: c.splice(at, temp, it); return it; } + + /// move element pos from this to other container, inserting before other_pos + iterator moveTo(iterator pos, container_type& other, iterator other_pos) { + other.splice(other_pos, c, pos); + return pos; + } + /// move element pos from other container into this one (sorted) + iterator moveFrom(iterator pos, container_type& other) { + iterator at = std::upper_bound(begin(), end(), *pos, comp); + c.splice(at, other, pos); + return pos; + } }; namespace detail { template struct ItemCostPair : std::pair { + typedef CostType cost_type; + ItemCostPair(const std::pair& other) : std::pair(other) {} ItemCostPair(std::pair&& other) @@ -101,7 +120,7 @@ struct ItemCostPair : std::pair { inline ValueType& value() { return this->first; } inline const ValueType& value() const { return this->first; } - inline const CostType& cost() const { return this->second; } + inline CostType cost() const { return this->second; } // comparison only considers cost constexpr bool operator<(const ItemCostPair& other) const { @@ -109,7 +128,7 @@ struct ItemCostPair : std::pair { } }; -} +} // namespace detail template >> @@ -117,7 +136,10 @@ class cost_ordered : public ordered, C { typedef ordered, Compare> base_type; public: - auto insert(const ValueType& value, const CostType& cost) { + auto insert(const ValueType& value, const CostType cost) { return base_type::insert(std::make_pair(value, cost)); } + auto insert(ValueType&& value, const CostType cost) { + return base_type::insert(std::make_pair(std::move(value), cost)); + } }; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index a2306f41..0f093ef4 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -203,7 +203,9 @@ public: enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD}; void restrictDirection(Direction dir); + virtual void reset() override; virtual void init(const planning_scene::PlanningSceneConstPtr &scene) override; + virtual bool computeForward(const InterfaceState& from) = 0; virtual bool computeBackward(const InterfaceState& to) = 0; @@ -285,6 +287,8 @@ public: PRIVATE_CLASS(Connecting) Connecting(const std::string& name); + virtual void reset() override; + virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0; void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 5781d86e..b30c2f8e 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -146,6 +146,7 @@ PIMPL_FUNCTIONS(ComputeBase) class PropagatingEitherWayPrivate : public ComputeBasePrivate { friend class PropagatingEitherWay; + std::list processed; // already processed states public: PropagatingEitherWay::Direction dir; @@ -166,14 +167,11 @@ public: const InterfaceState &fetchEndState(); protected: - // get informed when new start or end state becomes available - void newStartState(const std::list::iterator& it); - void newEndState(const std::list::iterator& it); // initialize properties from parent and/or state void initProperties(const InterfaceState &state); - - Interface::const_iterator next_start_state_; - Interface::const_iterator next_end_state_; + // drop states corresponding to failed (infinite-cost) trajectories + void dropFailedStarts(Interface::iterator state); + void dropFailedEnds(Interface::iterator state); }; PIMPL_FUNCTIONS(PropagatingEitherWay) @@ -209,6 +207,12 @@ PIMPL_FUNCTIONS(Generator) class ConnectingPrivate : public ComputeBasePrivate { friend class Connecting; + typedef std::pair StatePair; + struct StatePairLess { + bool operator()(const StatePair& x, const StatePair& y) const { + return x.first->priority() + x.second->priority() < y.first->priority() + y.second->priority(); + } + }; public: inline ConnectingPrivate(Connecting *me, const std::string &name); @@ -220,12 +224,13 @@ public: private: // get informed when new start or end state becomes available - void newStartState(const std::list::iterator& it); - void newEndState(const std::list::iterator& it); + void newStartState(Interface::iterator it, bool updated); + void newEndState(Interface::iterator it, bool updated); // initialize properties from parent and/or interface states void initProperties(const InterfaceState &start, const InterfaceState &end); - std::pair it_pairs_; + // ordered list of pending state pairs + ordered pending; }; PIMPL_FUNCTIONS(Connecting) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 3ff2a715..1ca83a00 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -74,8 +75,31 @@ 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 public: - // TODO turn this into priority queue + /** InterfaceStates are ordered according to two values: + * Depth of interlinked trajectory parts and accumulated trajectory costs along that path. + * Preference ordering considers high-depth first and within same depth, minimal cost paths. + */ + struct Priority : public std::pair { + Priority() : Priority(0, 0.0) {} + Priority(unsigned int depth, double cost) + : std::pair(depth, cost) {} + + inline unsigned int depth() const { return this->first; } + inline double cost() const { return this->second; } + + Priority operator+(const Priority& other) const { + return Priority(this->depth() + other.depth(), + this->cost() + other.cost()); + } + inline bool operator<(const Priority& other) const { + if (this->depth() == other.depth()) + return this->cost() < other.cost(); + else + return this->depth() > other.depth(); + } + }; typedef std::deque Solutions; /// create an InterfaceState from a planning scene @@ -91,6 +115,12 @@ public: PropertyMap& properties() { return properties_; } const PropertyMap& properties() const { return properties_; } + /// states are ordered by priority + inline bool operator<(const InterfaceState& other) const { + return this->priority_ < other.priority_; + } + inline const Priority& priority() const { return priority_; } + private: // these methods should be only called by SolutionBase::set[Start|End]State() inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); } @@ -101,50 +131,21 @@ private: PropertyMap properties_; Solutions incoming_trajectories_; Solutions outgoing_trajectories_; + Priority priority_; }; -/** Interface provides a list of InterfaceStates available as input for a stage. - * - * This is essentially an adaptor to a container class, to allow for notification - * of the interface's owner when new states become available - */ -class Interface : protected std::list { +/** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */ +class Interface : public ordered { public: - typedef std::list container_type; - typedef std::function NotifyFunction; + 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 - // and finally run the notify callback - container_type::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 - container_type::iterator clone(const InterfaceState &state); - - using container_type::value_type; - using container_type::reference; - using container_type::const_reference; - - using container_type::iterator; - using container_type::const_iterator; - using container_type::reverse_iterator; - using container_type::const_reverse_iterator; - - using container_type::empty; - using container_type::size; - using container_type::clear; - using container_type::front; - using container_type::back; - - using container_type::begin; - using container_type::cbegin; - using container_type::end; - using container_type::cend; - using container_type::rbegin; - using container_type::crbegin; - using container_type::rend; - using container_type::crend; + iterator clone(const InterfaceState &state); private: const NotifyFunction notify_; diff --git a/core/src/container.cpp b/core/src/container.cpp index 3ec1f436..c4a4b333 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -43,6 +43,9 @@ #include #include #include +#include + +using namespace std::placeholders; namespace moveit { namespace task_constructor { @@ -87,14 +90,14 @@ bool ContainerBasePrivate::compute() return static_cast(me_)->compute(); } -void ContainerBasePrivate::copyState(InterfaceState &external, - Stage &child, bool to_start) { +void ContainerBasePrivate::copyState(Interface::iterator external, + Stage &child, bool to_start, bool updated) { if (to_start) { - InterfaceState& internal = *child.pimpl()->starts()->clone(external); - internal_to_external_.insert(std::make_pair(&internal, &external)); + InterfaceState& internal = *child.pimpl()->starts()->clone(*external); + internal_to_external_.insert(std::make_pair(&internal, external)); } else { - InterfaceState& internal = *child.pimpl()->ends()->clone(external); - internal_to_external_.insert(std::make_pair(&internal, &external)); + InterfaceState& internal = *child.pimpl()->ends()->clone(*external); + internal_to_external_.insert(std::make_pair(&internal, external)); } } @@ -283,8 +286,8 @@ void SerialContainerPrivate::storeNewSolution(SerialSolution &&s) solution.setStartState(*it->second); } else { // spawn a new state in previous stage - InterfaceState& external = *prevEnds()->add(InterfaceState(*internal_from), NULL, &solution); - internal_to_external_.insert(std::make_pair(internal_from, &external)); + 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 @@ -294,8 +297,8 @@ void SerialContainerPrivate::storeNewSolution(SerialSolution &&s) solution.setEndState(*it->second); } else { // spawn a new state in next stage - InterfaceState& external = *nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); - internal_to_external_.insert(std::make_pair(internal_to, &external)); + 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 @@ -363,16 +366,16 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) // initialize starts_ and ends_ interfaces Stage* child = start->get(); if (child->pimpl()->starts()) - impl->starts_.reset(new Interface([impl, child](const Interface::iterator& external){ + impl->starts_.reset(new Interface([impl, child](Interface::iterator external, bool updated){ // new external state in our starts_ interface is copied to first child - impl->copyState(*external, *child, true); + impl->copyState(external, *child, true, updated); })); child = last->get(); if (child->pimpl()->ends()) - impl->ends_.reset(new Interface([impl, child](const Interface::iterator& external){ + impl->ends_.reset(new Interface([impl, child](Interface::iterator external, bool updated){ // new external state in our ends_ interface is copied to last child - impl->copyState(*external, *child, false); + impl->copyState(external, *child, false, updated); })); // validate connectivity of this @@ -466,12 +469,8 @@ void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg, ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name) : ContainerBasePrivate(me, name) { - starts_.reset(new Interface([me](const Interface::iterator& external){ - me->onNewStartState(*external); - })); - ends_.reset(new Interface([me](const Interface::iterator& external){ - me->onNewEndState(*external); - })); + starts_.reset(new Interface(std::bind(&ParallelContainerBase::onNewStartState, me, _1, _2))); + ends_.reset(new Interface(std::bind(&ParallelContainerBase::onNewEndState, me, _1, _2))); } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 351f4cd3..ff81ba6e 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -42,6 +42,7 @@ #include #include +using namespace std::placeholders; namespace moveit { namespace task_constructor { void InitStageException::push_back(const Stage &stage, const std::string &msg) @@ -257,32 +258,35 @@ PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *m { } +void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) { + // move infinite-cost states to processed list + if (std::isinf(state->priority().cost())) + starts_->moveTo(state, processed, processed.end()); +} +void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) { + // move infinite-cost states to processed list + if (std::isinf(state->priority().cost())) + ends_->moveTo(state, processed, processed.end()); +} + inline bool PropagatingEitherWayPrivate::hasStartState() const{ - return next_start_state_ != starts_->end(); + return starts_ && !starts_->empty(); } const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){ - if (!hasStartState()) - throw std::runtime_error("no new state for beginning available"); - - const InterfaceState& state= *next_start_state_; - ++next_start_state_; - - return state; + assert(hasStartState()); + // move state to processed list + return *starts_->moveTo(starts_->begin(), processed, processed.end()); } inline bool PropagatingEitherWayPrivate::hasEndState() const{ - return next_end_state_ != ends_->end(); + return ends_ && !ends_->empty(); } const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ - if(!hasEndState()) - throw std::runtime_error("no new state for ending available"); - - const InterfaceState& state= *next_end_state_; - ++next_end_state_; - - return state; + assert(hasEndState()); + // move state to processed list + return *ends_->moveTo(ends_->begin(), processed, processed.end()); } void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state) @@ -324,22 +328,6 @@ bool PropagatingEitherWayPrivate::compute() return countFailures(result); } -void PropagatingEitherWayPrivate::newStartState(const Interface::iterator &it) -{ - // we just appended a state to the list, but the iterator doesn't see it anymore - // so let's point it at the new one - if(next_start_state_ == starts_->end()) - --next_start_state_; -} - -void PropagatingEitherWayPrivate::newEndState(const Interface::iterator &it) -{ - // we just appended a state to the list, but the iterator doesn't see it anymore - // so let's point it at the new one - if(next_end_state_ == ends_->end()) - --next_end_state_; -} - PropagatingEitherWay::PropagatingEitherWay(const std::string &name) : PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name)) @@ -356,23 +344,17 @@ void PropagatingEitherWay::initInterface() { auto impl = pimpl(); if (impl->dir & PropagatingEitherWay::FORWARD) { - if (!impl->starts_) { // keep existing interface if possible - impl->starts_.reset(new Interface([impl](const Interface::iterator& it) { impl->newStartState(it); })); - impl->next_start_state_ = impl->starts_->begin(); - } + if (!impl->starts_) // keep existing interface if possible + impl->starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, impl, _1))); } else { impl->starts_.reset(); - impl->next_start_state_ = Interface::iterator(); } if (impl->dir & PropagatingEitherWay::BACKWARD) { - if (!impl->ends_) { // keep existing interface if possible - impl->ends_.reset(new Interface([impl](const Interface::iterator& it) { impl->newEndState(it); })); - impl->next_end_state_ = impl->ends_->end(); - } + if (!impl->ends_) // keep existing interface if possible + impl->ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, impl, _1))); } else { impl->ends_.reset(); - impl->next_end_state_ = Interface::iterator(); } } @@ -386,6 +368,12 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir initInterface(); } +void PropagatingEitherWay::reset() +{ + pimpl()->processed.clear(); + ComputeBase::reset(); +} + void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene) { ComputeBase::init(scene); @@ -393,14 +381,10 @@ void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &sce auto impl = pimpl(); // after being connected, restrict actual interface directions - if (!impl->nextStarts()) { + if (!impl->nextStarts()) impl->starts_.reset(); - impl->next_start_state_ = Interface::iterator(); - } - if (!impl->prevEnds()) { + if (!impl->prevEnds()) impl->ends_.reset(); - impl->next_end_state_ = Interface::iterator(); - } if (!impl->isConnected()) throw InitStageException(*this, "can neither send forwards nor backwards"); } @@ -433,7 +417,6 @@ PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, con { // indicate, that we don't accept new states from ends_ interface ends_.reset(); - next_end_state_ = Interface::iterator(); } @@ -452,7 +435,6 @@ PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me, { // indicate, that we don't accept new states from starts_ interface starts_.reset(); - next_start_state_ = Interface::iterator(); } @@ -510,23 +492,40 @@ void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name) : ComputeBasePrivate(me, name) { - starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); })); - ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); })); - it_pairs_ = std::make_pair(starts_->begin(), ends_->begin()); + starts_.reset(new Interface(std::bind(&ConnectingPrivate::newStartState, this, _1, _2))); + ends_.reset(new Interface(std::bind(&ConnectingPrivate::newEndState, this, _1, _2))); } -void ConnectingPrivate::newStartState(const Interface::iterator& it) +void ConnectingPrivate::newStartState(Interface::iterator it, bool updated) { - // TODO: need to handle the pairs iterator - if(it_pairs_.first == starts_->end()) - --it_pairs_.first; + if (!std::isfinite(it->priority().cost())) + return; + if (updated) { + // many pairs might be affected: sort + pending.sort(); + } else { // new state: insert all pairs with other interface + for (auto oit = ends_->begin(), oend = ends_->end(); oit != oend; ++oit) { + if (!std::isfinite(oit->priority().cost())) + break; + pending.insert(std::make_pair(it, oit)); + } + } } -void ConnectingPrivate::newEndState(const Interface::iterator& it) +void ConnectingPrivate::newEndState(Interface::iterator it, bool updated) { - // TODO: need to handle the pairs iterator properly - if(it_pairs_.second == ends_->end()) - --it_pairs_.second; + if (!std::isfinite(it->priority().cost())) + return; + if (updated) { + // many pairs might be affected: sort + pending.sort(); + } else { // new state: insert all pairs with other interface + for (auto oit = starts_->begin(), oend = starts_->end(); oit != oend; ++oit) { + if (!std::isfinite(oit->priority().cost())) + break; + pending.insert(std::make_pair(oit, it)); + } + } } void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end) @@ -538,15 +537,13 @@ void ConnectingPrivate::initProperties(const InterfaceState &start, const Interf } bool ConnectingPrivate::canCompute() const{ - // TODO: implement this properly - return it_pairs_.first != starts_->end() && - it_pairs_.second != ends_->end(); + return !pending.empty(); } bool ConnectingPrivate::compute() { - // TODO: implement this properly - const InterfaceState& from = *it_pairs_.first; - const InterfaceState& to = *(it_pairs_.second++); + const StatePair& top = pending.pop(); + const InterfaceState& from = *top.first; + const InterfaceState& to = *top.second; initProperties(from, to); return countFailures(static_cast(me_)->compute(from, to)); } @@ -557,6 +554,12 @@ Connecting::Connecting(const std::string &name) { } +void Connecting::reset() +{ + pimpl()->pending.clear(); + ComputeBase::reset(); +} + void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) { auto impl = pimpl(); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 629d7eec..eaa32393 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -59,29 +59,37 @@ Interface::Interface(const Interface::NotifyFunction ¬ify) : notify_(notify) {} +// Function used by sendForward()/sendBackward()/spawn() to create a new interface state Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing) { if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty()) throw std::runtime_error("expecting empty incoming/outgoing trajectories"); if (!state.scene()) throw std::runtime_error("expecting valid planning scene"); + // move state to a list node + std::list container; + auto it = container.insert(container.end(), std::move(state)); + // 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 - emplace_back(state); - iterator back = --end(); - // adjust subtrajectories ... - if (incoming) incoming->setEndState(*back); - if (outgoing) outgoing->setStartState(*back); - // ... before calling notify callback - if (notify_) notify_(back); - return back; + if (incoming) { + it->priority_ = InterfaceState::Priority(1, incoming->cost()); + incoming->setEndState(*it); + } else if (outgoing) { + it->priority_ = InterfaceState::Priority(1, outgoing->cost()); + outgoing->setStartState(*it); + } + // move list node into interface's state list (sorted by priority) + moveFrom(it, container); + // and finally call notify callback + if (notify_) notify_(it, false); + return it; } Interface::iterator Interface::clone(const InterfaceState &state) { - emplace_back(InterfaceState(state)); - iterator back = --end(); - if (notify_) notify_(back); - return back; + iterator it = insert(InterfaceState(state)); + if (notify_) notify_(it, false); + return it; }