From c2dd28abaeeb23af95b2b5a85bac5bf1bb52c0fd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 25 May 2018 15:07:12 +0200 Subject: [PATCH] rework storing of solutions - solutions_, failures_ as SolutionBaseConstPtrs in StagePrivate - replace processSolutions() / processFailures() by direct const-access to storage containers - generic sendForward(), sendBackward(), spawn(), connect() methods in StagePrivate - reuse StagePrivate's sendForward(), sendBackward(), spawn() in containers - store created InterfaceStates in StagePrivate::states_ - Interface: ordered (only store pointers) allows for common handling of states of valid and failure solutions - remove additional state+solution storages - containers: internal->external state mapping as InterfaceState* -> InterfaceState* --- .../moveit/task_constructor/container.h | 41 +--- .../moveit/task_constructor/container_p.h | 30 +-- core/include/moveit/task_constructor/stage.h | 30 ++- .../include/moveit/task_constructor/stage_p.h | 24 +- .../moveit/task_constructor/stages/connect.h | 28 +-- .../include/moveit/task_constructor/storage.h | 62 +++-- core/include/moveit/task_constructor/task.h | 5 +- core/src/container.cpp | 218 +++++------------- core/src/introspection.cpp | 29 +-- core/src/stage.cpp | 209 +++++++++-------- core/src/stages/connect.cpp | 88 +++---- core/src/storage.cpp | 52 ++--- core/src/task.cpp | 10 - .../src/local_task_model.cpp | 2 +- 14 files changed, 334 insertions(+), 494 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 54b03cf1..1149af9c 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -76,9 +76,6 @@ public: virtual bool canCompute() const = 0; virtual void compute() = 0; - size_t numFailures() const override { return 0; } - void processFailures(const SolutionProcessor&) const override {} - /// called by a (direct) child when a new solution becomes available virtual void onNewSolution(const SolutionBase& s) = 0; @@ -96,7 +93,6 @@ public: PRIVATE_CLASS(SerialContainer) SerialContainer(const std::string& name = "serial container"); - void reset() override; void init(const moveit::core::RobotModelConstPtr& robot_model) override; /// validate connectivity of children (after init() was done) @@ -105,9 +101,6 @@ public: bool canCompute() const override; void compute() override; - size_t numSolutions() const override; - void processSolutions(const SolutionProcessor &processor) const override; - protected: /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase &s) override; @@ -143,38 +136,23 @@ public: PRIVATE_CLASS(ParallelContainerBase) ParallelContainerBase(const std::string &name); - void reset() override; void init(const moveit::core::RobotModelConstPtr& robot_model) override; /// validate connectivity of children (after init() was done) void validateConnectivity() const override; - size_t numSolutions() const override; - void processSolutions(const SolutionProcessor &processor) const override; - size_t numFailures() const override; - void processFailures(const SolutionProcessor &processor) const override; - protected: ParallelContainerBase(ParallelContainerBasePrivate* impl); - /// method called if any child found a new (internal) solution - virtual void onNewSolution(const SolutionBase& s) override; - /// lift unmodified child solution (useful for simple filtering) - void liftSolution(const SolutionBase* solution) { - liftSolution(solution, solution->cost()); + inline void liftSolution(const SolutionBase& solution) { + liftSolution(solution, solution.cost()); } - /// lift child solution, but allow for modifying costs - void liftSolution(const SolutionBase* solution, double cost); + /// lift child solution to external interface, adapting the costs + void liftSolution(const SolutionBase& solution, double cost); /// spawn a new solution with given state as start and end void spawn(InterfaceState &&state, SubTrajectory&& trajectory); - /// convience method spawning an empty SubTrajectory with given cost - void spawn(InterfaceState &&state, double cost) { - SubTrajectory trajectory; - trajectory.setCost(cost); - spawn(std::move(state), std::move(trajectory)); - } /// propagate a solution forwards void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory); /// propagate a solution backwards @@ -193,6 +171,8 @@ public: bool canCompute() const override; void compute() override; + + void onNewSolution(const SolutionBase& s) override; }; @@ -213,6 +193,8 @@ public: void init(const moveit::core::RobotModelConstPtr& robot_model) override; bool canCompute() const override; void compute() override; + + void onNewSolution(const SolutionBase& s) override; }; @@ -242,10 +224,6 @@ class WrapperBasePrivate; * called when the child has generated a new solution. * The wrapper may reject the solution or create one or multiple derived solutions, * potentially adapting the cost, as well as its start and end states. - * - * Care needs to be taken to not modify pulled states, but only pushed ones! - * liftFor each new solution, liftSolution() should be called, which comes in various - * flavours to allow for handing in new states (or not). */ class WrapperBase : public ParallelContainerBase { @@ -267,9 +245,6 @@ public: protected: WrapperBase(WrapperBasePrivate* impl, Stage::pointer &&child = Stage::pointer()); - - /// called by a (direct) child when a new solution becomes available - void onNewSolution(const SolutionBase &s) override = 0; }; } } diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 3161cf68..ebd23ebe 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -124,7 +124,7 @@ protected: /// 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); /// lift solution from internal to external level - void liftSolution(SolutionBase& solution, + void liftSolution(SolutionBasePtr solution, const InterfaceState *internal_from, const InterfaceState *internal_to); auto& internalToExternalMap() { return internal_to_external_; } @@ -134,7 +134,7 @@ private: 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_; /* TODO: these interfaces don't need to be priority-sorted. * Introduce base class UnsortedInterface (which is a plain list) for this use case. */ @@ -174,9 +174,6 @@ private: void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end, InterfaceFlags accepted); - - // set of all solutions - ordered solutions_; }; PIMPL_FUNCTIONS(SerialContainer) @@ -214,32 +211,9 @@ public: // called by parent asking for pruning of this' interface void pruneInterface(InterfaceFlags accepted) override; - // grant access to protected methods in ParallelContainerBase - inline void spawn(InterfaceState &&state, SubTrajectory&& t) { - static_cast(me_)->spawn(std::move(state), std::move(t)); - } - inline void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) { - static_cast(me_)->sendForward(from, std::move(to), std::move(t)); - } - inline void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) { - static_cast(me_)->sendBackward(std::move(from), to, std::move(t)); - } - private: /// callback for new externally received states void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); - - // buffer for wrapped solutions - std::list wrapped_solutions_; - // buffer for newly created (not wrapped) solutions - std::list created_solutions_; - // buffer of created states (for use in created solutions) - std::list states_; - - // cost-ordered set of solutions (pointers into wrapped or created) - ordered solutions_; - // buffer for failures (pointers into wrapped or created) - std::list failures_; }; PIMPL_FUNCTIONS(ParallelContainerBase) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 5f232981..1bbd7d46 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -155,14 +155,6 @@ public: const ContainerBase* parent() const; const std::string& name() const; void setName(const std::string& name); - virtual size_t numSolutions() const = 0; - virtual size_t numFailures() const = 0; - - typedef std::function SolutionProcessor; - /// process all solutions in cost order, calling the callback for each of them - virtual void processSolutions(const SolutionProcessor &processor) const = 0; - /// process all failures, calling the callback for each of them - virtual void processFailures(const SolutionProcessor &processor) const = 0; typedef std::function SolutionCallback; typedef std::list SolutionCallbackList; @@ -171,6 +163,10 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); + const ordered& solutions() const; + const std::list& failures() const; + size_t numFailures() const; + /// get the stage's property map PropertyMap& properties(); const PropertyMap& properties() const { @@ -201,11 +197,6 @@ class ComputeBasePrivate; class ComputeBase : public Stage { public: PRIVATE_CLASS(ComputeBase) - void reset() override; - virtual size_t numSolutions() const override; - virtual size_t numFailures() const override; - void processSolutions(const SolutionProcessor &processor) const override; - void processFailures(const SolutionProcessor &processor) const override; protected: /// ComputeBase can only be instantiated by derived classes in stage.cpp @@ -227,7 +218,6 @@ public: }; void restrictDirection(Direction dir); - void reset() override; void init(const moveit::core::RobotModelConstPtr& robot_model) override; virtual void computeForward(const InterfaceState& from) = 0; @@ -329,13 +319,19 @@ public: void reset() override; virtual void compute(const InterfaceState& from, const InterfaceState& to) = 0; - void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory); + +protected: + /// register solution as a solution connecting states from -> to + void connect(const InterfaceState& from, const InterfaceState& to, SolutionBasePtr solution); + + /// convienency methods consuming a SubTrajectory + void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory) { + connect(from, to, std::make_shared(std::move(trajectory))); + } void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory, double cost) { trajectory.setCost(cost); connect(from, to, std::move(trajectory)); } - - void newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); }; } } diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index f9cb4d6e..d5abfaac 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -104,7 +104,14 @@ public: inline void setIntrospection(Introspection* introspection) { introspection_ = introspection; } void composePropertyErrorMsg(const std::string& name, std::ostream& os); - void newSolution(SolutionBase& current); + // methods to spawn new solutions + void sendForward(const InterfaceState& from, InterfaceState&& to, SolutionBasePtr solution); + void sendBackward(InterfaceState&& from, const InterfaceState& to, SolutionBasePtr solution); + void spawn(InterfaceState&& state, SolutionBasePtr solution); + void connect(const InterfaceState& from, const InterfaceState& to, SolutionBasePtr solution); + + bool storeSolution(const SolutionBasePtr& solution); + void newSolution(const SolutionBasePtr& solution); bool storeFailures() const { return introspection_ != nullptr; } protected: @@ -118,6 +125,11 @@ protected: // functions called for each new solution std::list solution_cbs_; + std::list states_; // storage for created states + ordered solutions_; + std::list failures_; + size_t num_failures_ = 0; // num of failures if not stored + private: // !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !! ContainerBase* parent_; // owning parent @@ -142,20 +154,13 @@ public: : StagePrivate(me, name) {} - // store trajectory in internal trajectories_ list - SubTrajectory& addTrajectory(SubTrajectory&& trajectory); - private: - ordered solutions_; - std::list failures_; - size_t num_failures_ = 0; // num of failures if not stored }; PIMPL_FUNCTIONS(ComputeBase) class PropagatingEitherWayPrivate : public ComputeBasePrivate { friend class PropagatingEitherWay; - std::list processed; // already processed states public: PropagatingEitherWay::Direction required_interface_dirs_; @@ -242,9 +247,6 @@ public: bool canCompute() const override; void compute() override; - void connect(const robot_trajectory::RobotTrajectoryPtr& t, - const InterfaceStatePair& state_pair, double cost); - private: // get informed when new start or end state becomes available template diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index f8c48785..426dfce9 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -49,6 +49,14 @@ MOVEIT_CLASS_FORWARD(RobotState) namespace moveit { namespace task_constructor { namespace stages { +/** Connect arbitrary InterfaceStates by motion planning + * + * The states may differ in various planning groups. + * To connect both states, the planners provided for individual sub groups are applied in the + * specified order. Each planner only plan for joints within the corresponding planning group. + * Finally, an attempt is made to merge the sub trajectories of individual planning results. + * If this fails, the sequential planning result is returned. + */ class Connect : public Connecting { protected: bool compatible(const InterfaceState &from_state, const InterfaceState &to_state) const override; @@ -57,11 +65,11 @@ public: typedef std::vector> GroupPlannerVector; Connect(const std::string& name, const GroupPlannerVector& planners); - void setTimeout(const ros::Duration& timeout){ + void setTimeout(const ros::Duration& timeout) { setProperty("timeout", timeout.toSec()); } - void setPathConstraints(moveit_msgs::Constraints path_constraints){ + void setPathConstraints(moveit_msgs::Constraints path_constraints) { setProperty("path_constraints", std::move(path_constraints)); } @@ -69,23 +77,17 @@ public: void init(const moveit::core::RobotModelConstPtr& robot_model) override; void compute(const InterfaceState &from, const InterfaceState &to) override; - size_t numSolutions() const override; - void processSolutions(const SolutionProcessor &processor) const override; - void processFailures(const SolutionProcessor &processor) const override; - protected: - SolutionBase* storeSequential(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes); - robot_trajectory::RobotTrajectoryConstPtr merge(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state); + SolutionSequencePtr makeSequential(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes, const InterfaceState& from, const InterfaceState& to); + SubTrajectoryPtr merge(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes, + const moveit::core::RobotState& state); protected: GroupPlannerVector planner_; moveit::core::JointModelGroupPtr merged_jmg_; - // TODO: ComputeBase should handle any SolutionBase -> shared_ptr std::list subsolutions_; - std::list solutions_; std::list states_; }; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 28abb387..781008d1 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -134,18 +134,47 @@ private: }; +/// compare InterfaceState* by value +struct InterfaceStateLess { + inline bool operator()(const InterfaceState* x, const InterfaceState* y) const { + return *x < *y; + } +}; + /** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */ -class Interface : public ordered { +class Interface : public ordered { + typedef ordered base_type; + public: + // iterators providing convinient access to stored InterfaceState + class iterator : public base_type::iterator { + public: + iterator(base_type::iterator other) : base_type::iterator(other) {} + + InterfaceState& operator*() const noexcept + { return *base_type::iterator::operator*(); } + + InterfaceState* operator->() const noexcept + { return base_type::iterator::operator*(); } + }; + class const_iterator : public base_type::const_iterator { + public: + const_iterator(base_type::const_iterator other) : base_type::const_iterator(other) {} + const_iterator(base_type::iterator other) : base_type::const_iterator(other) {} + + const InterfaceState& operator*() const noexcept + { return *base_type::const_iterator::operator*(); } + + const InterfaceState* operator->() const noexcept + { return base_type::const_iterator::operator*(); } + }; + enum Direction { FORWARD, BACKWARD, START=FORWARD, END=BACKWARD }; 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 - iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing); - - /// clone an existing InterfaceState, but without its incoming/outgoing connections - iterator clone(const InterfaceState &state); + /// add a new InterfaceState + void add(InterfaceState &state); /// remove a state from the interface and return it as a one-element list container_type remove(iterator it); @@ -158,17 +187,15 @@ private: // restrict access to some functions to ensure consistency // (we need to set/unset InterfaceState::owner_) - using ordered::moveTo; - using ordered::moveFrom; - using ordered::insert; - using ordered::erase; - using ordered::remove_if; + using base_type::moveTo; + using base_type::moveFrom; + using base_type::insert; + using base_type::erase; + using base_type::remove_if; }; class StagePrivate; - - /// abstract base class for solutions (primitive and sequences) class SolutionBase { public: @@ -179,13 +206,15 @@ public: inline const InterfaceState::Solutions& trajectories() const; inline void setStartState(const InterfaceState& state){ - assert(start_ == NULL); // only allow setting once (by Stage) + // only allow setting once (by Stage) + assert(start_ == NULL || start_ == &state); start_ = &state; const_cast(state).addOutgoing(this); } inline void setEndState(const InterfaceState& state){ - assert(end_ == NULL); // only allow setting once (by Stage) + // only allow setting once (by Stage) + assert(end_ == NULL || end_ == &state); end_ = &state; const_cast(state).addIncoming(this); } @@ -233,6 +262,7 @@ private: const InterfaceState* start_ = nullptr; const InterfaceState* end_ = nullptr; }; +MOVEIT_CLASS_FORWARD(SolutionBase) /// SubTrajectory connects interface states of ComputeStages @@ -253,6 +283,7 @@ private: // actual trajectory, might be empty robot_trajectory::RobotTrajectoryConstPtr trajectory_; }; +MOVEIT_CLASS_FORWARD(SubTrajectory) /** Sequence of individual sub solutions @@ -283,6 +314,7 @@ private: /// series of sub solutions container_type subsolutions_; }; +MOVEIT_CLASS_FORWARD(SolutionSequence) template <> inline const InterfaceState::Solutions& SolutionBase::trajectories() const { diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 8ca6ad20..2a8529f3 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -107,8 +107,9 @@ public: /// print current task state (number of found solutions and propagated states) to std::cout void printState(std::ostream &os = std::cout) const; - size_t numSolutions() const override; - void processSolutions(const ContainerBase::SolutionProcessor &processor) const override; + size_t numSolutions() const { return solutions().size(); } + const ordered& solutions() const { return stages()->solutions(); } + const std::list& failures() const { return stages()->failures(); } /// publish all top-level solutions void publishAllSolutions(bool wait = true); diff --git a/core/src/container.cpp b/core/src/container.cpp index 3616e2f5..22f1d60d 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -106,35 +106,42 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa return; // create a clone of external state within target interface (child's starts() or ends()) - InterfaceState& internal = *target->clone(*external); + auto internal = states_.insert(states_.end(), InterfaceState(*external)); + target->add(*internal); // and remember the mapping between them - internal_to_external_.insert(std::make_pair(&internal, external)); + internal_to_external_.insert(std::make_pair(&*internal, &*external)); } -void ContainerBasePrivate::liftSolution(SolutionBase& solution, +void ContainerBasePrivate::liftSolution(SolutionBasePtr 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)); - } + if (!storeSolution(solution)) + return; - // 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)); - } + auto findOrCreateExternal = [this](const InterfaceState *internal, bool &created) -> InterfaceState* { + auto it = internal_to_external_.find(internal); + if (it != internal_to_external_.end()) + return it->second; + + InterfaceState* external = &*states_.insert(states_.end(), InterfaceState(*internal)); + internal_to_external_.insert(std::make_pair(internal, external)); + created = true; + return external; + }; + bool created_from = false; + bool created_to = false; + InterfaceState *external_from = findOrCreateExternal(internal_from, created_from); + InterfaceState *external_to = findOrCreateExternal(internal_to, created_to); + + // connect solution to start/end state + solution->setStartState(*external_from); + solution->setEndState(*external_to); + + // spawn created states in external interfaces + if (created_from) prevEnds()->add(*external_from); + if (created_to) nextStarts()->add(*external_to); + + newSolution(solution); } @@ -162,7 +169,8 @@ bool ContainerBase::traverseRecursively(const ContainerBase::StageCallback &proc bool ContainerBase::insert(Stage::pointer &&stage, int before) { StagePrivate *impl = stage->pimpl(); - if (impl->parent() != nullptr || numSolutions() != 0) { + if (impl->parent() != nullptr || + !stage->solutions().empty() || !stage->failures().empty()) { ROS_ERROR("cannot re-parent stage"); return false; } @@ -334,7 +342,7 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) traverse(current, std::ref(outgoing), trace); // collect (and sort) all solutions spanning from start to end of this container - ordered sorted; + ordered sorted; SolutionSequence::container_type solution; solution.reserve(children.size()); for (auto& in : incoming.solutions) { @@ -353,7 +361,7 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) // insert outgoing solutions in normal order solution.insert(solution.end(), out.first.begin(), out.first.end()); // store solution in sorted list - sorted.insert(SolutionSequence(std::move(solution), prio.cost(), impl)); + sorted.insert(std::make_shared(std::move(solution), prio.cost(), impl)); } else if (prio.depth() > 1) { // update state priorities along the whole partial solution path updateStateCosts(in.first, prio); @@ -363,12 +371,9 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) } } - // store new solutions (in sorted) - for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) { - auto inserted = impl->solutions_.insert(std::move(*it)); - impl->liftSolution(*inserted, inserted->internalStart(), inserted->internalEnd()); - impl->newSolution(*inserted); - } + // finally store + announce new solutions to external interface + for (const auto &solution : sorted) + impl->liftSolution(solution, solution->internalStart(), solution->internalEnd()); } @@ -379,17 +384,6 @@ SerialContainer::SerialContainer(const std::string &name) : SerialContainer(new SerialContainerPrivate(this, name)) {} -void SerialContainer::reset() -{ - auto impl = pimpl(); - - // clear queues - impl->solutions_.clear(); - - // recursively reset children - ContainerBase::reset(); -} - SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name) : ContainerBasePrivate(me, name) {} @@ -641,18 +635,6 @@ void SerialContainer::compute() } } -size_t SerialContainer::numSolutions() const -{ - return pimpl()->solutions_.size(); -} - -void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &processor) const -{ - for(const SolutionBase& s : pimpl()->solutions_) - if (!processor(s)) - break; -} - template void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb, SolutionSequence::container_type &trace, double trace_cost) @@ -738,19 +720,6 @@ ParallelContainerBase::ParallelContainerBase(const std::string &name) : ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {} -void ParallelContainerBase::reset() -{ - // recursively reset children - ContainerBase::reset(); - // clear buffers - auto impl = pimpl(); - impl->solutions_.clear(); - impl->failures_.clear(); - impl->wrapped_solutions_.clear(); - impl->created_solutions_.clear(); - impl->states_.clear(); -} - /* States received by the container need to be copied to all children's pull interfaces. * States generated by children can be directly forwarded into the container's push interfaces. */ @@ -805,114 +774,26 @@ void ParallelContainerBase::validateConnectivity() const if (errors) throw errors; } -size_t ParallelContainerBase::numSolutions() const -{ - return pimpl()->solutions_.size(); -} - -void ParallelContainerBase::processSolutions(const Stage::SolutionProcessor &processor) const -{ - for(const SolutionBase* s : pimpl()->solutions_) - if (!processor(*s)) - break; -} - -size_t ParallelContainerBase::numFailures() const -{ - return pimpl()->failures_.size(); -} - -void ParallelContainerBase::processFailures(const Stage::SolutionProcessor &processor) const -{ - for(const SolutionBase* f : pimpl()->failures_) - if (!processor(*f)) - break; -} - -void ParallelContainerBase::onNewSolution(const SolutionBase& s) -{ - liftSolution(&s); -} - -void ParallelContainerBase::liftSolution(const SolutionBase* solution, double cost) +void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost) { auto impl = pimpl(); - // create new WrappedSolution instance - auto wit = impl->wrapped_solutions_.insert(impl->wrapped_solutions_.end(), WrappedSolution(impl, solution, cost)); - - if (wit->isFailure()) { - wit->setStartState(*solution->start()); - wit->setEndState(*solution->end()); - impl->failures_.push_back(&*wit); - } else { - impl->solutions_.insert(&*wit); - impl->liftSolution(*wit, solution->start(), solution->end()); - } - impl->newSolution(*wit); + impl->liftSolution(std::make_shared(impl, &solution, cost), + solution.start(), solution.end()); } void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t) { - auto impl = pimpl(); - assert(impl->prevEnds() && impl->nextStarts()); - - // store newly created solution (otherwise it's lost) - auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t)); - - if (it->isFailure()) { - // attach state (different for start / end) to trajectory - auto state_it = impl->states_.insert(impl->states_.end(), InterfaceState(state)); - it->setStartState(*state_it); - state_it = impl->states_.insert(impl->states_.end(), std::move(state)); - it->setEndState(*state_it); - impl->failures_.push_back(&*it); - } else { - // directly spawn states into push interfaces - impl->prevEnds()->add(InterfaceState(state), NULL, &*it); - impl->nextStarts()->add(std::move(state), &*it, NULL); - impl->solutions_.insert(&*it); - } - impl->newSolution(*it); + pimpl()->StagePrivate::spawn(std::move(state), std::make_shared(std::move(t))); } void ParallelContainerBase::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) { - auto impl = pimpl(); - assert(impl->nextStarts()); - - // store newly created solution (otherwise it's lost) - auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t)); - it->setStartState(from); - - if (it->isFailure()) { - auto state_it = impl->states_.insert(impl->states_.end(), std::move(to)); - it->setEndState(*state_it); - impl->failures_.push_back(&*it); - } else { - impl->nextStarts()->add(std::move(to), &*it, NULL); - impl->solutions_.insert(&*it); - } - impl->newSolution(*it); + pimpl()->StagePrivate::sendForward(from, std::move(to), std::make_shared(std::move(t))); } void ParallelContainerBase::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) { - auto impl = pimpl(); - assert(impl->prevEnds()); - - // store newly created solution (otherwise it's lost) - auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t)); - it->setEndState(to); - - if (it->isFailure()) { - auto state_it = impl->states_.insert(impl->states_.end(), std::move(from)); - it->setStartState(*state_it); - impl->failures_.push_back(&*it); - } else { - impl->prevEnds()->add(std::move(from), NULL, &*it); - impl->solutions_.insert(&*it); - } - impl->newSolution(*it); + pimpl()->StagePrivate::sendBackward(std::move(from), to, std::make_shared(std::move(t))); } @@ -978,6 +859,10 @@ void Alternatives::compute() } } +void Alternatives::onNewSolution(const SolutionBase& s) +{ + liftSolution(s); +} void Fallbacks::reset() { @@ -1016,6 +901,11 @@ void Fallbacks::compute() } } +void Fallbacks::onNewSolution(const SolutionBase& s) +{ + liftSolution(s); +} + MergerPrivate::MergerPrivate(Merger *me, const std::string &name) : ParallelContainerBasePrivate(me, name) @@ -1150,7 +1040,7 @@ void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) // generate target state planning_scene::PlanningScenePtr to = from->scene()->diff(); to->setCurrentState(t.trajectory()->getLastWayPoint()); - ParallelContainerBasePrivate::sendForward(*from, InterfaceState(to), std::move(t)); + StagePrivate::sendForward(*from, InterfaceState(to), std::make_shared(std::move(t))); } void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) @@ -1158,7 +1048,7 @@ void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) // generate target state planning_scene::PlanningScenePtr from = to->scene()->diff(); from->setCurrentState(t.trajectory()->getFirstWayPoint()); - ParallelContainerBasePrivate::sendBackward(InterfaceState(from), *to, std::move(t)); + StagePrivate::sendBackward(InterfaceState(from), *to, std::make_shared(std::move(t))); } void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s) diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 6c43ccbf..a3ceba0d 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -152,24 +152,16 @@ void Introspection::publishSolution(const SolutionBase &s) void Introspection::publishAllSolutions(bool wait) { - moveit_task_constructor_msgs::Solution msg; - - Stage::SolutionProcessor processor = [this, &msg, wait](const SolutionBase& s) { - msg.sub_solution.clear(); - msg.sub_trajectory.clear(); - fillSolution(msg, s); - impl->solution_publisher_.publish(msg); + for (const auto& solution : impl->task_.solutions()) { + publishSolution(*solution); if (wait) { std::cout << "Press to continue ..." << std::endl; int ch = getchar(); if (ch == 'q' || ch == 'Q') - return false; + break; } - return true; }; - - impl->task_.processSolutions(processor); } bool Introspection::getSolution(moveit_task_constructor_msgs::GetSolution::Request &req, @@ -204,18 +196,13 @@ uint32_t Introspection::solutionId(const SolutionBase& s) void Introspection::fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::StageStatistics& s) { // successfull solutions - Stage::SolutionProcessor solutionProcessor = [this, &s](const SolutionBase& solution) { - s.solved.push_back(solutionId(solution)); - return true; - }; - stage.processSolutions(solutionProcessor); + for (const auto& solution : stage.solutions()) + s.solved.push_back(solutionId(*solution)); // failed solution attempts - solutionProcessor = [this, &s](const SolutionBase& solution) { - s.failed.push_back(solutionId(solution)); - return true; - }; - stage.processFailures(solutionProcessor); + for (const auto& solution : stage.failures()) + s.failed.push_back(solutionId(*solution)); + s.num_failed = stage.numFailures(); } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 5c96bf58..ed758b2f 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -83,19 +83,96 @@ InterfaceFlags StagePrivate::interfaceFlags() const return f; } -void StagePrivate::newSolution(SolutionBase &solution) +bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { - solution.setCreator(this); - + solution->setCreator(this); if (introspection_) - introspection_->registerSolution(solution); + introspection_->registerSolution(*solution); + if (solution->isFailure()) { + ++num_failures_; + if (!storeFailures()) + return false; // drop solution + failures_.push_back(solution); + } else { + solutions_.insert(solution); + } + return true; +} + +void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, SolutionBasePtr solution) +{ + assert(nextStarts()); + if (!storeSolution(solution)) + return; // solution dropped + + auto to_it = states_.insert(states_.end(), std::move(to)); + + solution->setStartState(from); + solution->setEndState(*to_it); + + if (!solution->isFailure()) + nextStarts()->add(*to_it); + + newSolution(solution); +} + +void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, SolutionBasePtr solution) +{ + assert(prevEnds()); + if (!storeSolution(solution)) + return; // solution dropped + + auto from_it = states_.insert(states_.end(), std::move(from)); + + solution->setStartState(*from_it); + solution->setEndState(to); + + if (!solution->isFailure()) + prevEnds()->add(*from_it); + + newSolution(solution); +} + +void StagePrivate::spawn(InterfaceState&& state, SolutionBasePtr solution) +{ + assert(prevEnds() && nextStarts()); + if (!storeSolution(solution)) + return; // solution dropped + + auto from = states_.insert(states_.end(), InterfaceState(state)); // copy + auto to = states_.insert(states_.end(), std::move(state)); + + solution->setStartState(*from); + solution->setEndState(*to); + + if (!solution->isFailure()) { + prevEnds()->add(*from); + nextStarts()->add(*to); + } + + newSolution(solution); +} + +void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, SolutionBasePtr solution) +{ + if (!storeSolution(solution)) + return; // solution dropped + + solution->setStartState(from); + solution->setEndState(to); + + newSolution(solution); +} + +void StagePrivate::newSolution(const SolutionBasePtr& solution) +{ // call solution callbacks for both, valid solutions and failures for (const auto& cb : solution_cbs_) - cb(solution); + cb(*solution); if (parent()) - parent()->onNewSolution(solution); + parent()->onNewSolution(*solution); } Stage::Stage(StagePrivate *impl) @@ -120,6 +197,11 @@ Stage::operator const StagePrivate*() const { void Stage::reset() { auto impl = pimpl(); + // clear solutions + associated states + impl->solutions_.clear(); + impl->failures_.clear(); + impl->num_failures_ = 0u; + impl->states_.clear(); // clear pull interfaces if (impl->starts_) impl->starts_->clear(); if (impl->ends_) impl->ends_->clear(); @@ -172,6 +254,22 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) pimpl()->solution_cbs_.erase(which); } +const ordered& Stage::solutions() const +{ + return pimpl()->solutions_; +} + +const std::list& Stage::failures() const +{ + return pimpl()->failures_; +} + +size_t Stage::numFailures() const +{ + return pimpl()->num_failures_; +} + + PropertyMap &Stage::properties() { return pimpl()->properties_; @@ -232,7 +330,7 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { } // trajectories os << std::setw(5) << direction(impl) - << std::setw(3) << impl.me()->numSolutions() + << std::setw(3) << impl.solutions_.size() << std::setw(5) << direction(impl); // ends for (const InterfaceConstPtr& i : {impl.ends(), impl.nextStarts()}) { @@ -245,55 +343,12 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { return os; } -SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) { - if (!trajectory.isFailure()) - return *solutions_.insert(std::move(trajectory)); - - ++num_failures_; - if (storeFailures()) - return *failures_.insert(failures_.end(), std::move(trajectory)); - - return trajectory; -} - ComputeBase::ComputeBase(ComputeBasePrivate *impl) : Stage(impl) { } -size_t ComputeBase::numSolutions() const { - return pimpl()->solutions_.size(); -} - -size_t ComputeBase::numFailures() const -{ - return pimpl()->num_failures_; -} - -void ComputeBase::processSolutions(const Stage::SolutionProcessor &processor) const -{ - for (const auto& s : pimpl()->solutions_) - if (!processor(s)) - return; -} - -void ComputeBase::processFailures(const Stage::SolutionProcessor &processor) const -{ - for (const auto& s : pimpl()->failures_) - if (!processor(s)) - return; -} - -void ComputeBase::reset() { - auto impl = pimpl(); - impl->solutions_.clear(); - impl->failures_.clear(); - impl->num_failures_ = 0u; - Stage::reset(); -} - - PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name) : ComputeBasePrivate(me, name), required_interface_dirs_(dir) { @@ -340,14 +395,12 @@ InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const } void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) { - // move infinite-cost states to processed list if (std::isinf(state->priority().cost())) - processed.splice(processed.end(), starts_->remove(state)); + starts_->remove(state); } void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) { - // move infinite-cost states to processed list if (std::isinf(state->priority().cost())) - processed.splice(processed.end(), ends_->remove(state)); + ends_->remove(state); } inline bool PropagatingEitherWayPrivate::hasStartState() const{ @@ -356,9 +409,7 @@ inline bool PropagatingEitherWayPrivate::hasStartState() const{ const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){ assert(hasStartState()); - // move state to end of processed list - processed.splice(processed.end(), starts_->remove(starts_->begin())); - return processed.back(); + return *starts_->remove(starts_->begin()).front(); } inline bool PropagatingEitherWayPrivate::hasEndState() const{ @@ -367,9 +418,7 @@ inline bool PropagatingEitherWayPrivate::hasEndState() const{ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ assert(hasEndState()); - // move state to processed list - processed.splice(processed.end(), ends_->remove(ends_->begin())); - return processed.back(); + return *ends_->remove(ends_->begin()).front(); } bool PropagatingEitherWayPrivate::canCompute() const @@ -416,12 +465,6 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir impl->initInterface(impl->required_interface_dirs_); } -void PropagatingEitherWay::reset() -{ - pimpl()->processed.clear(); - ComputeBase::reset(); -} - void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model) { Stage::init(robot_model); @@ -440,21 +483,13 @@ void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_mo void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) { - auto impl = pimpl(); - SubTrajectory &trajectory = impl->addTrajectory(std::move(t)); - trajectory.setStartState(from); - impl->nextStarts()->add(std::move(to), &trajectory, NULL); - impl->newSolution(trajectory); + pimpl()->sendForward(from, std::move(to), std::make_shared(std::move(t))); } void PropagatingEitherWay::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) { - auto impl = pimpl(); - SubTrajectory& trajectory = impl->addTrajectory(std::move(t)); - trajectory.setEndState(to); - impl->prevEnds()->add(std::move(from), NULL, &trajectory); - impl->newSolution(trajectory); + pimpl()->sendBackward(std::move(from), to, std::make_shared(std::move(t))); } @@ -520,15 +555,7 @@ Generator::Generator(const std::string &name) void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) { - assert(state.incomingTrajectories().empty() && - state.outgoingTrajectories().empty()); - assert(!t.trajectory()); - - auto impl = pimpl(); - SubTrajectory& trajectory = impl->addTrajectory(std::move(t)); - impl->prevEnds()->add(InterfaceState(state), NULL, &trajectory); - impl->nextStarts()->add(std::move(state), &trajectory, NULL); - impl->newSolution(trajectory); + pimpl()->spawn(std::move(state), std::make_shared(std::move(t))); } @@ -607,7 +634,7 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) pending.sort(); } else { // new state: insert all pairs with other interface InterfacePtr other_interface = pullInterface(other); - for (auto oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { + for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { if (!std::isfinite(oit->priority().cost())) break; if (static_cast(me_)->compatible(*it, *oit)) @@ -663,14 +690,8 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta return true; } -void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) { - newSolution(from, to, pimpl()->addTrajectory(std::move(t))); -} - -void Connecting::newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) { - solution.setStartState(from); - solution.setEndState(to); - pimpl()->newSolution(solution); +void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SolutionBasePtr s) { + pimpl()->connect(from, to, s); } std::ostream& operator<<(std::ostream& os, const Stage& stage) { diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 3f47eb53..cb77ec27 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -57,7 +57,6 @@ void Connect::reset() { Connecting::reset(); merged_jmg_.reset(); - solutions_.clear(); subsolutions_.clear(); states_.clear(); } @@ -148,6 +147,7 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) { std::vector intermediate_scenes; intermediate_scenes.push_back(start); + bool success = false; std::vector positions; for (const GroupPlannerVector::value_type& pair : planner_) { // set intermediate goal state @@ -158,39 +158,36 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) { end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions); robot_trajectory::RobotTrajectoryPtr trajectory; - if (!pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints)) + success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); + sub_trajectories.push_back(trajectory); // include failed trajectory + + if (!success) break; - sub_trajectories.push_back(trajectory); // continue from reached state start = end; } - SolutionBase* solution = nullptr; - if (sub_trajectories.size() != planner_.size()) { // error during sequential planning - // push back a dummy solution to also show the target scene of the failed attempt - sub_trajectories.push_back(robot_trajectory::RobotTrajectoryPtr()); - solution = storeSequential(sub_trajectories, intermediate_scenes); + SolutionBasePtr solution; + if (!success) { // error during sequential planning + solution = makeSequential(sub_trajectories, intermediate_scenes, from, to); solution->markAsFailure(); } else { - auto t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState()); - if (t) { - connect(from, to, SubTrajectory(t)); - return; - } - // merging failed, store sequentially - solution = storeSequential(sub_trajectories, intermediate_scenes); + solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState()); + if (!solution) // merging failed, store sequentially + solution = makeSequential(sub_trajectories, intermediate_scenes, from, to); } - - newSolution(from, to, *solution); + connect(from, to, solution); } -SolutionBase* Connect::storeSequential(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes) +SolutionSequencePtr Connect::makeSequential(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes, + const InterfaceState &from, const InterfaceState &to) { assert(sub_trajectories.size() + 1 == intermediate_scenes.size()); auto scene_it = intermediate_scenes.begin(); planning_scene::PlanningScenePtr start = *scene_it; + const InterfaceState* state = &from; SolutionSequence::container_type sub_solutions; for (const auto &sub : sub_trajectories) { @@ -202,58 +199,37 @@ SolutionBase* Connect::storeSequential(const std::vector(std::move(sub_solutions)); } -robot_trajectory::RobotTrajectoryConstPtr Connect::merge(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state) +SubTrajectoryPtr Connect::merge(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes, + const moveit::core::RobotState& state) { // no need to merge if there is only a single sub trajectory if (sub_trajectories.size() == 1) - return sub_trajectories[0]; + return std::make_shared(sub_trajectories[0]); auto jmg = merged_jmg_.get(); assert(jmg); robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg); + if (!trajectory) + return SubTrajectoryPtr(); + // TODO: check merged trajectory for collisions - return trajectory; -} -size_t Connect::numSolutions() const -{ - return solutions_.size() + Connecting::numSolutions(); -} - -void Connect::processSolutions(const Stage::SolutionProcessor& processor) const -{ - // TODO: This is not nice, but necessary to process simple SubTrajectory + SolutionSequence - for (const auto& s : solutions_) { - if (s.isFailure()) continue; - if (!processor(s)) - break; - } - Connecting::processSolutions(processor); -} - -void Connect::processFailures(const Stage::SolutionProcessor &processor) const -{ - // TODO: This is not nice, but necessary to process simple SubTrajectory + SolutionSequence - for (const auto& s : solutions_) { - if (!s.isFailure()) continue; - if (!processor(s)) - break; - } - Connecting::processFailures(processor); + return std::make_shared(trajectory); } } } } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 8ba03538..e7cb53cd 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -51,7 +51,7 @@ InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps) } InterfaceState::InterfaceState(const InterfaceState &other) - : scene_(other.scene_), properties_(other.properties_) + : scene_(other.scene_), properties_(other.properties_), priority_(other.priority_) { } @@ -76,41 +76,35 @@ 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 in InterfaceState"); +// Announce a new InterfaceState +void Interface::add(InterfaceState &state) { + // require valid scene + assert(state.scene()); + // incoming and outgoing must not contain elements both + assert(state.incomingTrajectories().empty() || state.outgoingTrajectories().empty()); + // if non-empty, incoming or outgoing should have exactly one solution element + assert(state.incomingTrajectories().empty() || state.incomingTrajectories().size() == 1); + assert(state.outgoingTrajectories().empty() || state.outgoingTrajectories().size() == 1); + // state can only be added once to an interface + assert(state.owner_ == nullptr); // move state to a list node - std::list container; - auto it = container.insert(container.end(), std::move(state)); - assert(it->owner_ == nullptr); // state can only be added once to an interface + std::list container; + Interface::iterator it = container.insert(container.end(), &state); 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) { - it->priority_ = InterfaceState::Priority(1, incoming->cost()); - incoming->setEndState(*it); - } else if (outgoing) { - it->priority_ = InterfaceState::Priority(1, outgoing->cost()); - outgoing->setStartState(*it); - } + // if either incoming or outgoing is defined, derive priority from there + if (!state.incomingTrajectories().empty()) + it->priority_ = InterfaceState::Priority(1, state.incomingTrajectories().front()->cost()); + else if (!state.outgoingTrajectories().empty()) + it->priority_ = InterfaceState::Priority(1, state.outgoingTrajectories().front()->cost()); + else // otherwise, assume priority was well defined before + assert(it->priority_ >= InterfaceState::Priority(1, 0.0)); + // 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) -{ - iterator it = insert(InterfaceState(state)); - it->owner_ = this; - if (notify_) notify_(it, false); - return it; } Interface::container_type Interface::remove(iterator it) @@ -124,7 +118,7 @@ Interface::container_type Interface::remove(iterator 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; }); + auto it = std::find_if(begin(), end(), [state](const InterfaceState* other) { return state == other; }); // state should be part of the interface assert(it != end()); state->priority_ = priority; diff --git a/core/src/task.cpp b/core/src/task.cpp index 40468a40..58c6bf50 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -224,16 +224,6 @@ bool Task::plan() return numSolutions() > 0; } -size_t Task::numSolutions() const -{ - return stages()->numSolutions(); -} - -void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) const -{ - stages()->processSolutions(processor); -} - void Task::publishAllSolutions(bool wait) { enableIntrospection(true); diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index 5cf12f42..8855c59d 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -148,7 +148,7 @@ QVariant LocalTaskModel::data(const QModelIndex &index, int role) const case Qt::DisplayRole: switch (index.column()) { case 0: return QString::fromStdString(n->name()); - case 1: return (uint)n->me()->numSolutions(); + case 1: return (uint)n->me()->solutions().size(); case 2: return 0; } break;