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<InterfaceState*> (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*
This commit is contained in:
Robert Haschke 2018-05-25 15:07:12 +02:00 committed by v4hn
parent ae6b86c7d7
commit c2dd28abae
14 changed files with 334 additions and 494 deletions

View File

@ -76,9 +76,6 @@ public:
virtual bool canCompute() const = 0; virtual bool canCompute() const = 0;
virtual void compute() = 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 /// called by a (direct) child when a new solution becomes available
virtual void onNewSolution(const SolutionBase& s) = 0; virtual void onNewSolution(const SolutionBase& s) = 0;
@ -96,7 +93,6 @@ public:
PRIVATE_CLASS(SerialContainer) PRIVATE_CLASS(SerialContainer)
SerialContainer(const std::string& name = "serial container"); SerialContainer(const std::string& name = "serial container");
void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done) /// validate connectivity of children (after init() was done)
@ -105,9 +101,6 @@ public:
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
size_t numSolutions() const override;
void processSolutions(const SolutionProcessor &processor) const override;
protected: protected:
/// called by a (direct) child when a new solution becomes available /// called by a (direct) child when a new solution becomes available
void onNewSolution(const SolutionBase &s) override; void onNewSolution(const SolutionBase &s) override;
@ -143,38 +136,23 @@ public:
PRIVATE_CLASS(ParallelContainerBase) PRIVATE_CLASS(ParallelContainerBase)
ParallelContainerBase(const std::string &name); ParallelContainerBase(const std::string &name);
void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done) /// validate connectivity of children (after init() was done)
void validateConnectivity() const override; 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: protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl); 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) /// lift unmodified child solution (useful for simple filtering)
void liftSolution(const SolutionBase* solution) { inline void liftSolution(const SolutionBase& solution) {
liftSolution(solution, solution->cost()); liftSolution(solution, solution.cost());
} }
/// lift child solution, but allow for modifying costs /// lift child solution to external interface, adapting the costs
void liftSolution(const SolutionBase* solution, double cost); void liftSolution(const SolutionBase& solution, double cost);
/// spawn a new solution with given state as start and end /// spawn a new solution with given state as start and end
void spawn(InterfaceState &&state, SubTrajectory&& trajectory); 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 /// propagate a solution forwards
void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory); void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory);
/// propagate a solution backwards /// propagate a solution backwards
@ -193,6 +171,8 @@ public:
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
void onNewSolution(const SolutionBase& s) override;
}; };
@ -213,6 +193,8 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override; bool canCompute() const override;
void compute() 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. * called when the child has generated a new solution.
* The wrapper may reject the solution or create one or multiple derived solutions, * 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. * 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 class WrapperBase : public ParallelContainerBase
{ {
@ -267,9 +245,6 @@ public:
protected: protected:
WrapperBase(WrapperBasePrivate* impl, Stage::pointer &&child = Stage::pointer()); 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;
}; };
} } } }

View File

@ -124,7 +124,7 @@ protected:
/// copy external_state to a child's interface and remember the link in internal_to map /// 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); void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
/// lift solution from internal to external level /// lift solution from internal to external level
void liftSolution(SolutionBase& solution, void liftSolution(SolutionBasePtr solution,
const InterfaceState *internal_from, const InterfaceState *internal_to); const InterfaceState *internal_from, const InterfaceState *internal_to);
auto& internalToExternalMap() { return internal_to_external_; } auto& internalToExternalMap() { return internal_to_external_; }
@ -134,7 +134,7 @@ private:
container_type children_; container_type children_;
// map start/end states of children (internal) to corresponding states in our external interfaces // map start/end states of children (internal) to corresponding states in our external interfaces
std::map<const InterfaceState*, Interface::iterator> internal_to_external_; std::map<const InterfaceState*, InterfaceState*> internal_to_external_;
/* TODO: these interfaces don't need to be priority-sorted. /* TODO: these interfaces don't need to be priority-sorted.
* Introduce base class UnsortedInterface (which is a plain list) for this use case. */ * 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, void pruneInterfaces(container_type::const_iterator first,
container_type::const_iterator end, container_type::const_iterator end,
InterfaceFlags accepted); InterfaceFlags accepted);
// set of all solutions
ordered<SolutionSequence> solutions_;
}; };
PIMPL_FUNCTIONS(SerialContainer) PIMPL_FUNCTIONS(SerialContainer)
@ -214,32 +211,9 @@ public:
// called by parent asking for pruning of this' interface // called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override; void pruneInterface(InterfaceFlags accepted) override;
// grant access to protected methods in ParallelContainerBase
inline void spawn(InterfaceState &&state, SubTrajectory&& t) {
static_cast<ParallelContainerBase*>(me_)->spawn(std::move(state), std::move(t));
}
inline void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
static_cast<ParallelContainerBase*>(me_)->sendForward(from, std::move(to), std::move(t));
}
inline void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) {
static_cast<ParallelContainerBase*>(me_)->sendBackward(std::move(from), to, std::move(t));
}
private: private:
/// callback for new externally received states /// callback for new externally received states
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
// buffer for wrapped solutions
std::list<WrappedSolution> wrapped_solutions_;
// buffer for newly created (not wrapped) solutions
std::list<SubTrajectory> created_solutions_;
// buffer of created states (for use in created solutions)
std::list<InterfaceState> states_;
// cost-ordered set of solutions (pointers into wrapped or created)
ordered<SolutionBase*> solutions_;
// buffer for failures (pointers into wrapped or created)
std::list<SolutionBase*> failures_;
}; };
PIMPL_FUNCTIONS(ParallelContainerBase) PIMPL_FUNCTIONS(ParallelContainerBase)

View File

@ -155,14 +155,6 @@ public:
const ContainerBase* parent() const; const ContainerBase* parent() const;
const std::string& name() const; const std::string& name() const;
void setName(const std::string& name); void setName(const std::string& name);
virtual size_t numSolutions() const = 0;
virtual size_t numFailures() const = 0;
typedef std::function<bool(const SolutionBase&)> 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<void(const SolutionBase &s)> SolutionCallback; typedef std::function<void(const SolutionBase &s)> SolutionCallback;
typedef std::list<SolutionCallback> SolutionCallbackList; typedef std::list<SolutionCallback> SolutionCallbackList;
@ -171,6 +163,10 @@ public:
/// remove function callback /// remove function callback
void removeSolutionCallback(SolutionCallbackList::const_iterator which); void removeSolutionCallback(SolutionCallbackList::const_iterator which);
const ordered<SolutionBaseConstPtr>& solutions() const;
const std::list<SolutionBaseConstPtr>& failures() const;
size_t numFailures() const;
/// get the stage's property map /// get the stage's property map
PropertyMap& properties(); PropertyMap& properties();
const PropertyMap& properties() const { const PropertyMap& properties() const {
@ -201,11 +197,6 @@ class ComputeBasePrivate;
class ComputeBase : public Stage { class ComputeBase : public Stage {
public: public:
PRIVATE_CLASS(ComputeBase) 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: protected:
/// ComputeBase can only be instantiated by derived classes in stage.cpp /// ComputeBase can only be instantiated by derived classes in stage.cpp
@ -227,7 +218,6 @@ public:
}; };
void restrictDirection(Direction dir); void restrictDirection(Direction dir);
void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
virtual void computeForward(const InterfaceState& from) = 0; virtual void computeForward(const InterfaceState& from) = 0;
@ -329,13 +319,19 @@ public:
void reset() override; void reset() override;
virtual void compute(const InterfaceState& from, const InterfaceState& to) = 0; 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<SubTrajectory>(std::move(trajectory)));
}
void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory, double cost) { void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory, double cost) {
trajectory.setCost(cost); trajectory.setCost(cost);
connect(from, to, std::move(trajectory)); connect(from, to, std::move(trajectory));
} }
void newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
}; };
} } } }

View File

@ -104,7 +104,14 @@ public:
inline void setIntrospection(Introspection* introspection) { introspection_ = introspection; } inline void setIntrospection(Introspection* introspection) { introspection_ = introspection; }
void composePropertyErrorMsg(const std::string& name, std::ostream& os); 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; } bool storeFailures() const { return introspection_ != nullptr; }
protected: protected:
@ -118,6 +125,11 @@ protected:
// functions called for each new solution // functions called for each new solution
std::list<Stage::SolutionCallback> solution_cbs_; std::list<Stage::SolutionCallback> solution_cbs_;
std::list<InterfaceState> states_; // storage for created states
ordered<SolutionBaseConstPtr> solutions_;
std::list<SolutionBaseConstPtr> failures_;
size_t num_failures_ = 0; // num of failures if not stored
private: private:
// !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !! // !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !!
ContainerBase* parent_; // owning parent ContainerBase* parent_; // owning parent
@ -142,20 +154,13 @@ public:
: StagePrivate(me, name) : StagePrivate(me, name)
{} {}
// store trajectory in internal trajectories_ list
SubTrajectory& addTrajectory(SubTrajectory&& trajectory);
private: private:
ordered<SubTrajectory> solutions_;
std::list<SubTrajectory> failures_;
size_t num_failures_ = 0; // num of failures if not stored
}; };
PIMPL_FUNCTIONS(ComputeBase) PIMPL_FUNCTIONS(ComputeBase)
class PropagatingEitherWayPrivate : public ComputeBasePrivate { class PropagatingEitherWayPrivate : public ComputeBasePrivate {
friend class PropagatingEitherWay; friend class PropagatingEitherWay;
std::list<Interface::value_type> processed; // already processed states
public: public:
PropagatingEitherWay::Direction required_interface_dirs_; PropagatingEitherWay::Direction required_interface_dirs_;
@ -242,9 +247,6 @@ public:
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
void connect(const robot_trajectory::RobotTrajectoryPtr& t,
const InterfaceStatePair& state_pair, double cost);
private: private:
// get informed when new start or end state becomes available // get informed when new start or end state becomes available
template<Interface::Direction other> template<Interface::Direction other>

View File

@ -49,6 +49,14 @@ MOVEIT_CLASS_FORWARD(RobotState)
namespace moveit { namespace task_constructor { namespace stages { 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 { class Connect : public Connecting {
protected: protected:
bool compatible(const InterfaceState &from_state, const InterfaceState &to_state) const override; bool compatible(const InterfaceState &from_state, const InterfaceState &to_state) const override;
@ -57,11 +65,11 @@ public:
typedef std::vector<std::pair<std::string, solvers::PlannerInterfacePtr>> GroupPlannerVector; typedef std::vector<std::pair<std::string, solvers::PlannerInterfacePtr>> GroupPlannerVector;
Connect(const std::string& name, const GroupPlannerVector& planners); Connect(const std::string& name, const GroupPlannerVector& planners);
void setTimeout(const ros::Duration& timeout){ void setTimeout(const ros::Duration& timeout) {
setProperty("timeout", timeout.toSec()); 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)); setProperty("path_constraints", std::move(path_constraints));
} }
@ -69,23 +77,17 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
void compute(const InterfaceState &from, const InterfaceState &to) 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: protected:
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories, SolutionSequencePtr makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes); const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes, const InterfaceState& from, const InterfaceState& to);
robot_trajectory::RobotTrajectoryConstPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories, SubTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes, const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const moveit::core::RobotState& state); const moveit::core::RobotState& state);
protected: protected:
GroupPlannerVector planner_; GroupPlannerVector planner_;
moveit::core::JointModelGroupPtr merged_jmg_; moveit::core::JointModelGroupPtr merged_jmg_;
// TODO: ComputeBase should handle any SolutionBase -> shared_ptr
std::list<SubTrajectory> subsolutions_; std::list<SubTrajectory> subsolutions_;
std::list<SolutionSequence> solutions_;
std::list<InterfaceState> states_; std::list<InterfaceState> states_;
}; };

View File

@ -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. */ /** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */
class Interface : public ordered<InterfaceState> { class Interface : public ordered<InterfaceState*, InterfaceStateLess> {
typedef ordered<InterfaceState*, InterfaceStateLess> base_type;
public: 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 }; enum Direction { FORWARD, BACKWARD, START=FORWARD, END=BACKWARD };
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
iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing); void add(InterfaceState &state);
/// clone an existing InterfaceState, but without its incoming/outgoing connections
iterator clone(const InterfaceState &state);
/// remove a state from the interface and return it as a one-element list /// remove a state from the interface and return it as a one-element list
container_type remove(iterator it); container_type remove(iterator it);
@ -158,17 +187,15 @@ private:
// restrict access to some functions to ensure consistency // restrict access to some functions to ensure consistency
// (we need to set/unset InterfaceState::owner_) // (we need to set/unset InterfaceState::owner_)
using ordered<InterfaceState>::moveTo; using base_type::moveTo;
using ordered<InterfaceState>::moveFrom; using base_type::moveFrom;
using ordered<InterfaceState>::insert; using base_type::insert;
using ordered<InterfaceState>::erase; using base_type::erase;
using ordered<InterfaceState>::remove_if; using base_type::remove_if;
}; };
class StagePrivate; class StagePrivate;
/// abstract base class for solutions (primitive and sequences) /// abstract base class for solutions (primitive and sequences)
class SolutionBase { class SolutionBase {
public: public:
@ -179,13 +206,15 @@ public:
inline const InterfaceState::Solutions& trajectories() const; inline const InterfaceState::Solutions& trajectories() const;
inline void setStartState(const InterfaceState& state){ 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; 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); // only allow setting once (by Stage) // only allow setting once (by Stage)
assert(end_ == NULL || end_ == &state);
end_ = &state; end_ = &state;
const_cast<InterfaceState&>(state).addIncoming(this); const_cast<InterfaceState&>(state).addIncoming(this);
} }
@ -233,6 +262,7 @@ private:
const InterfaceState* start_ = nullptr; const InterfaceState* start_ = nullptr;
const InterfaceState* end_ = nullptr; const InterfaceState* end_ = nullptr;
}; };
MOVEIT_CLASS_FORWARD(SolutionBase)
/// SubTrajectory connects interface states of ComputeStages /// SubTrajectory connects interface states of ComputeStages
@ -253,6 +283,7 @@ private:
// actual trajectory, might be empty // actual trajectory, might be empty
robot_trajectory::RobotTrajectoryConstPtr trajectory_; robot_trajectory::RobotTrajectoryConstPtr trajectory_;
}; };
MOVEIT_CLASS_FORWARD(SubTrajectory)
/** Sequence of individual sub solutions /** Sequence of individual sub solutions
@ -283,6 +314,7 @@ private:
/// series of sub solutions /// series of sub solutions
container_type subsolutions_; container_type subsolutions_;
}; };
MOVEIT_CLASS_FORWARD(SolutionSequence)
template <> inline template <> inline
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const { const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const {

View File

@ -107,8 +107,9 @@ public:
/// print current task state (number of found solutions and propagated states) to std::cout /// print current task state (number of found solutions and propagated states) to std::cout
void printState(std::ostream &os = std::cout) const; void printState(std::ostream &os = std::cout) const;
size_t numSolutions() const override; size_t numSolutions() const { return solutions().size(); }
void processSolutions(const ContainerBase::SolutionProcessor &processor) const override; const ordered<SolutionBaseConstPtr>& solutions() const { return stages()->solutions(); }
const std::list<SolutionBaseConstPtr>& failures() const { return stages()->failures(); }
/// publish all top-level solutions /// publish all top-level solutions
void publishAllSolutions(bool wait = true); void publishAllSolutions(bool wait = true);

View File

@ -106,35 +106,42 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
return; return;
// create a clone of external state within target interface (child's starts() or ends()) // 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 // 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) const InterfaceState *internal_from, const InterfaceState *internal_to)
{ {
// add solution to existing or new start state if (!storeSolution(solution))
auto it = internal_to_external_.find(internal_from); return;
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));
}
// add solution to existing or new end state auto findOrCreateExternal = [this](const InterfaceState *internal, bool &created) -> InterfaceState* {
it = internal_to_external_.find(internal_to); auto it = internal_to_external_.find(internal);
if (it != internal_to_external_.end()) { if (it != internal_to_external_.end())
// connect solution to existing start state return it->second;
solution.setEndState(*it->second);
} else { InterfaceState* external = &*states_.insert(states_.end(), InterfaceState(*internal));
// spawn a new state in next stage internal_to_external_.insert(std::make_pair(internal, external));
Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); created = true;
internal_to_external_.insert(std::make_pair(internal_to, external)); 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) bool ContainerBase::insert(Stage::pointer &&stage, int before)
{ {
StagePrivate *impl = stage->pimpl(); 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"); ROS_ERROR("cannot re-parent stage");
return false; return false;
} }
@ -334,7 +342,7 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
traverse<Interface::FORWARD>(current, std::ref(outgoing), trace); traverse<Interface::FORWARD>(current, std::ref(outgoing), trace);
// collect (and sort) all solutions spanning from start to end of this container // collect (and sort) all solutions spanning from start to end of this container
ordered<SolutionSequence> sorted; ordered<SolutionSequencePtr> sorted;
SolutionSequence::container_type solution; SolutionSequence::container_type solution;
solution.reserve(children.size()); solution.reserve(children.size());
for (auto& in : incoming.solutions) { for (auto& in : incoming.solutions) {
@ -353,7 +361,7 @@ void SerialContainer::onNewSolution(const SolutionBase &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 // store solution in sorted list
sorted.insert(SolutionSequence(std::move(solution), prio.cost(), impl)); sorted.insert(std::make_shared<SolutionSequence>(std::move(solution), prio.cost(), impl));
} else if (prio.depth() > 1) { } else if (prio.depth() > 1) {
// update state priorities along the whole partial solution path // update state priorities along the whole partial solution path
updateStateCosts(in.first, prio); updateStateCosts(in.first, prio);
@ -363,12 +371,9 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
} }
} }
// store new solutions (in sorted) // finally store + announce new solutions to external interface
for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) { for (const auto &solution : sorted)
auto inserted = impl->solutions_.insert(std::move(*it)); impl->liftSolution(solution, solution->internalStart(), solution->internalEnd());
impl->liftSolution(*inserted, inserted->internalStart(), inserted->internalEnd());
impl->newSolution(*inserted);
}
} }
@ -379,17 +384,6 @@ SerialContainer::SerialContainer(const std::string &name)
: SerialContainer(new SerialContainerPrivate(this, 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) SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name)
: ContainerBasePrivate(me, 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 <Interface::Direction dir> template <Interface::Direction dir>
void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb, void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb,
SolutionSequence::container_type &trace, double trace_cost) SolutionSequence::container_type &trace, double trace_cost)
@ -738,19 +720,6 @@ ParallelContainerBase::ParallelContainerBase(const std::string &name)
: ParallelContainerBase(new ParallelContainerBasePrivate(this, 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 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. * 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; if (errors) throw errors;
} }
size_t ParallelContainerBase::numSolutions() const void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost)
{
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)
{ {
auto impl = pimpl(); auto impl = pimpl();
// create new WrappedSolution instance impl->liftSolution(std::make_shared<WrappedSolution>(impl, &solution, cost),
auto wit = impl->wrapped_solutions_.insert(impl->wrapped_solutions_.end(), WrappedSolution(impl, solution, cost)); solution.start(), solution.end());
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);
} }
void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t) void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t)
{ {
auto impl = pimpl(); pimpl()->StagePrivate::spawn(std::move(state), std::make_shared<SubTrajectory>(std::move(t)));
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);
} }
void ParallelContainerBase::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) void ParallelContainerBase::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t)
{ {
auto impl = pimpl(); pimpl()->StagePrivate::sendForward(from, std::move(to), std::make_shared<SubTrajectory>(std::move(t)));
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);
} }
void ParallelContainerBase::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) void ParallelContainerBase::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t)
{ {
auto impl = pimpl(); pimpl()->StagePrivate::sendBackward(std::move(from), to, std::make_shared<SubTrajectory>(std::move(t)));
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);
} }
@ -978,6 +859,10 @@ void Alternatives::compute()
} }
} }
void Alternatives::onNewSolution(const SolutionBase& s)
{
liftSolution(s);
}
void Fallbacks::reset() 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) MergerPrivate::MergerPrivate(Merger *me, const std::string &name)
: ParallelContainerBasePrivate(me, name) : ParallelContainerBasePrivate(me, name)
@ -1150,7 +1040,7 @@ void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from)
// generate target state // generate target state
planning_scene::PlanningScenePtr to = from->scene()->diff(); planning_scene::PlanningScenePtr to = from->scene()->diff();
to->setCurrentState(t.trajectory()->getLastWayPoint()); to->setCurrentState(t.trajectory()->getLastWayPoint());
ParallelContainerBasePrivate::sendForward(*from, InterfaceState(to), std::move(t)); StagePrivate::sendForward(*from, InterfaceState(to), std::make_shared<SubTrajectory>(std::move(t)));
} }
void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to)
@ -1158,7 +1048,7 @@ void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to)
// generate target state // generate target state
planning_scene::PlanningScenePtr from = to->scene()->diff(); planning_scene::PlanningScenePtr from = to->scene()->diff();
from->setCurrentState(t.trajectory()->getFirstWayPoint()); from->setCurrentState(t.trajectory()->getFirstWayPoint());
ParallelContainerBasePrivate::sendBackward(InterfaceState(from), *to, std::move(t)); StagePrivate::sendBackward(InterfaceState(from), *to, std::make_shared<SubTrajectory>(std::move(t)));
} }
void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s) void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s)

View File

@ -152,24 +152,16 @@ void Introspection::publishSolution(const SolutionBase &s)
void Introspection::publishAllSolutions(bool wait) void Introspection::publishAllSolutions(bool wait)
{ {
moveit_task_constructor_msgs::Solution msg; for (const auto& solution : impl->task_.solutions()) {
publishSolution(*solution);
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);
if (wait) { if (wait) {
std::cout << "Press <Enter> to continue ..." << std::endl; std::cout << "Press <Enter> to continue ..." << std::endl;
int ch = getchar(); int ch = getchar();
if (ch == 'q' || ch == 'Q') if (ch == 'q' || ch == 'Q')
return false; break;
} }
return true;
}; };
impl->task_.processSolutions(processor);
} }
bool Introspection::getSolution(moveit_task_constructor_msgs::GetSolution::Request &req, 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) void Introspection::fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::StageStatistics& s)
{ {
// successfull solutions // successfull solutions
Stage::SolutionProcessor solutionProcessor = [this, &s](const SolutionBase& solution) { for (const auto& solution : stage.solutions())
s.solved.push_back(solutionId(solution)); s.solved.push_back(solutionId(*solution));
return true;
};
stage.processSolutions(solutionProcessor);
// failed solution attempts // failed solution attempts
solutionProcessor = [this, &s](const SolutionBase& solution) { for (const auto& solution : stage.failures())
s.failed.push_back(solutionId(solution)); s.failed.push_back(solutionId(*solution));
return true;
};
stage.processFailures(solutionProcessor);
s.num_failed = stage.numFailures(); s.num_failed = stage.numFailures();
} }

View File

@ -83,19 +83,96 @@ InterfaceFlags StagePrivate::interfaceFlags() const
return f; return f;
} }
void StagePrivate::newSolution(SolutionBase &solution) bool StagePrivate::storeSolution(const SolutionBasePtr& solution)
{ {
solution.setCreator(this); solution->setCreator(this);
if (introspection_) 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 // call solution callbacks for both, valid solutions and failures
for (const auto& cb : solution_cbs_) for (const auto& cb : solution_cbs_)
cb(solution); cb(*solution);
if (parent()) if (parent())
parent()->onNewSolution(solution); parent()->onNewSolution(*solution);
} }
Stage::Stage(StagePrivate *impl) Stage::Stage(StagePrivate *impl)
@ -120,6 +197,11 @@ Stage::operator const StagePrivate*() const {
void Stage::reset() void Stage::reset()
{ {
auto impl = pimpl(); auto impl = pimpl();
// clear solutions + associated states
impl->solutions_.clear();
impl->failures_.clear();
impl->num_failures_ = 0u;
impl->states_.clear();
// clear pull interfaces // clear pull interfaces
if (impl->starts_) impl->starts_->clear(); if (impl->starts_) impl->starts_->clear();
if (impl->ends_) impl->ends_->clear(); if (impl->ends_) impl->ends_->clear();
@ -172,6 +254,22 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which)
pimpl()->solution_cbs_.erase(which); pimpl()->solution_cbs_.erase(which);
} }
const ordered<SolutionBaseConstPtr>& Stage::solutions() const
{
return pimpl()->solutions_;
}
const std::list<SolutionBaseConstPtr>& Stage::failures() const
{
return pimpl()->failures_;
}
size_t Stage::numFailures() const
{
return pimpl()->num_failures_;
}
PropertyMap &Stage::properties() PropertyMap &Stage::properties()
{ {
return pimpl()->properties_; return pimpl()->properties_;
@ -232,7 +330,7 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
} }
// trajectories // trajectories
os << std::setw(5) << direction<READS_START, WRITES_PREV_END>(impl) os << std::setw(5) << direction<READS_START, WRITES_PREV_END>(impl)
<< std::setw(3) << impl.me()->numSolutions() << std::setw(3) << impl.solutions_.size()
<< std::setw(5) << direction<READS_END, WRITES_NEXT_START>(impl); << std::setw(5) << direction<READS_END, WRITES_NEXT_START>(impl);
// ends // ends
for (const InterfaceConstPtr& i : {impl.ends(), impl.nextStarts()}) { for (const InterfaceConstPtr& i : {impl.ends(), impl.nextStarts()}) {
@ -245,55 +343,12 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
return os; 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) ComputeBase::ComputeBase(ComputeBasePrivate *impl)
: Stage(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) PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name)
: ComputeBasePrivate(me, name), required_interface_dirs_(dir) : ComputeBasePrivate(me, name), required_interface_dirs_(dir)
{ {
@ -340,14 +395,12 @@ InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
} }
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) { void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
// move infinite-cost states to processed list
if (std::isinf(state->priority().cost())) if (std::isinf(state->priority().cost()))
processed.splice(processed.end(), starts_->remove(state)); starts_->remove(state);
} }
void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) { void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) {
// move infinite-cost states to processed list
if (std::isinf(state->priority().cost())) if (std::isinf(state->priority().cost()))
processed.splice(processed.end(), ends_->remove(state)); ends_->remove(state);
} }
inline bool PropagatingEitherWayPrivate::hasStartState() const{ inline bool PropagatingEitherWayPrivate::hasStartState() const{
@ -356,9 +409,7 @@ inline bool PropagatingEitherWayPrivate::hasStartState() const{
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){ const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
assert(hasStartState()); assert(hasStartState());
// move state to end of processed list return *starts_->remove(starts_->begin()).front();
processed.splice(processed.end(), starts_->remove(starts_->begin()));
return processed.back();
} }
inline bool PropagatingEitherWayPrivate::hasEndState() const{ inline bool PropagatingEitherWayPrivate::hasEndState() const{
@ -367,9 +418,7 @@ inline bool PropagatingEitherWayPrivate::hasEndState() const{
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
assert(hasEndState()); assert(hasEndState());
// move state to processed list return *ends_->remove(ends_->begin()).front();
processed.splice(processed.end(), ends_->remove(ends_->begin()));
return processed.back();
} }
bool PropagatingEitherWayPrivate::canCompute() const bool PropagatingEitherWayPrivate::canCompute() const
@ -416,12 +465,6 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir
impl->initInterface(impl->required_interface_dirs_); impl->initInterface(impl->required_interface_dirs_);
} }
void PropagatingEitherWay::reset()
{
pimpl()->processed.clear();
ComputeBase::reset();
}
void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model) void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
Stage::init(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, void PropagatingEitherWay::sendForward(const InterfaceState& from,
InterfaceState&& to, InterfaceState&& to,
SubTrajectory&& t) { SubTrajectory&& t) {
auto impl = pimpl(); pimpl()->sendForward(from, std::move(to), std::make_shared<SubTrajectory>(std::move(t)));
SubTrajectory &trajectory = impl->addTrajectory(std::move(t));
trajectory.setStartState(from);
impl->nextStarts()->add(std::move(to), &trajectory, NULL);
impl->newSolution(trajectory);
} }
void PropagatingEitherWay::sendBackward(InterfaceState&& from, void PropagatingEitherWay::sendBackward(InterfaceState&& from,
const InterfaceState& to, const InterfaceState& to,
SubTrajectory&& t) { SubTrajectory&& t) {
auto impl = pimpl(); pimpl()->sendBackward(std::move(from), to, std::make_shared<SubTrajectory>(std::move(t)));
SubTrajectory& trajectory = impl->addTrajectory(std::move(t));
trajectory.setEndState(to);
impl->prevEnds()->add(std::move(from), NULL, &trajectory);
impl->newSolution(trajectory);
} }
@ -520,15 +555,7 @@ Generator::Generator(const std::string &name)
void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) void Generator::spawn(InterfaceState&& state, SubTrajectory&& t)
{ {
assert(state.incomingTrajectories().empty() && pimpl()->spawn(std::move(state), std::make_shared<SubTrajectory>(std::move(t)));
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);
} }
@ -607,7 +634,7 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated)
pending.sort(); pending.sort();
} else { // new state: insert all pairs with other interface } else { // new state: insert all pairs with other interface
InterfacePtr other_interface = pullInterface(other); 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())) if (!std::isfinite(oit->priority().cost()))
break; break;
if (static_cast<Connecting*>(me_)->compatible(*it, *oit)) if (static_cast<Connecting*>(me_)->compatible(*it, *oit))
@ -663,14 +690,8 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
return true; return true;
} }
void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) { void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SolutionBasePtr s) {
newSolution(from, to, pimpl()->addTrajectory(std::move(t))); pimpl()->connect(from, to, s);
}
void Connecting::newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) {
solution.setStartState(from);
solution.setEndState(to);
pimpl()->newSolution(solution);
} }
std::ostream& operator<<(std::ostream& os, const Stage& stage) { std::ostream& operator<<(std::ostream& os, const Stage& stage) {

View File

@ -57,7 +57,6 @@ void Connect::reset()
{ {
Connecting::reset(); Connecting::reset();
merged_jmg_.reset(); merged_jmg_.reset();
solutions_.clear();
subsolutions_.clear(); subsolutions_.clear();
states_.clear(); states_.clear();
} }
@ -148,6 +147,7 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
std::vector<planning_scene::PlanningScenePtr> intermediate_scenes; std::vector<planning_scene::PlanningScenePtr> intermediate_scenes;
intermediate_scenes.push_back(start); intermediate_scenes.push_back(start);
bool success = false;
std::vector<double> positions; std::vector<double> positions;
for (const GroupPlannerVector::value_type& pair : planner_) { for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state // set intermediate goal state
@ -158,39 +158,36 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions); end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions);
robot_trajectory::RobotTrajectoryPtr trajectory; 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; break;
sub_trajectories.push_back(trajectory);
// continue from reached state // continue from reached state
start = end; start = end;
} }
SolutionBase* solution = nullptr; SolutionBasePtr solution;
if (sub_trajectories.size() != planner_.size()) { // error during sequential planning if (!success) { // error during sequential planning
// push back a dummy solution to also show the target scene of the failed attempt solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
sub_trajectories.push_back(robot_trajectory::RobotTrajectoryPtr());
solution = storeSequential(sub_trajectories, intermediate_scenes);
solution->markAsFailure(); solution->markAsFailure();
} else { } else {
auto t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState()); solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
if (t) { if (!solution) // merging failed, store sequentially
connect(from, to, SubTrajectory(t)); solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
return;
}
// merging failed, store sequentially
solution = storeSequential(sub_trajectories, intermediate_scenes);
} }
connect(from, to, solution);
newSolution(from, to, *solution);
} }
SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories, SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes) const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const InterfaceState &from, const InterfaceState &to)
{ {
assert(sub_trajectories.size() + 1 == intermediate_scenes.size()); assert(sub_trajectories.size() + 1 == intermediate_scenes.size());
auto scene_it = intermediate_scenes.begin(); auto scene_it = intermediate_scenes.begin();
planning_scene::PlanningScenePtr start = *scene_it; planning_scene::PlanningScenePtr start = *scene_it;
const InterfaceState* state = &from;
SolutionSequence::container_type sub_solutions; SolutionSequence::container_type sub_solutions;
for (const auto &sub : sub_trajectories) { for (const auto &sub : sub_trajectories) {
@ -202,58 +199,37 @@ SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::Robot
sub_solutions.push_back(&*inserted); sub_solutions.push_back(&*inserted);
// provide meaningful start/end states // provide meaningful start/end states
states_.emplace_back(InterfaceState(start)); subsolutions_.back().setStartState(*state);
subsolutions_.back().setStartState(states_.back()); state = &*states_.insert(states_.end(), InterfaceState(end));
states_.emplace_back(InterfaceState(end));
subsolutions_.back().setEndState(states_.back()); if (sub_solutions.size() < sub_trajectories.size())
subsolutions_.back().setEndState(*state);
else
subsolutions_.back().setEndState(to);
start = end; start = end;
} }
solutions_.emplace_back(SolutionSequence(std::move(sub_solutions))); return std::make_shared<SolutionSequence>(std::move(sub_solutions));
return &solutions_.back();
} }
robot_trajectory::RobotTrajectoryConstPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories, SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes, const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const moveit::core::RobotState& state) const moveit::core::RobotState& state)
{ {
// no need to merge if there is only a single sub trajectory // no need to merge if there is only a single sub trajectory
if (sub_trajectories.size() == 1) if (sub_trajectories.size() == 1)
return sub_trajectories[0]; return std::make_shared<SubTrajectory>(sub_trajectories[0]);
auto jmg = merged_jmg_.get(); auto jmg = merged_jmg_.get();
assert(jmg); assert(jmg);
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg); robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
if (!trajectory)
return SubTrajectoryPtr();
// TODO: check merged trajectory for collisions // TODO: check merged trajectory for collisions
return trajectory;
}
size_t Connect::numSolutions() const return std::make_shared<SubTrajectory>(trajectory);
{
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);
} }
} } } } } }

View File

@ -51,7 +51,7 @@ InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps)
} }
InterfaceState::InterfaceState(const InterfaceState &other) 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 &notify)
: notify_(notify) : notify_(notify)
{} {}
// Function used by sendForward()/sendBackward()/spawn() to create a new interface state // Announce a new InterfaceState
Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing) { void Interface::add(InterfaceState &state) {
if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty()) // require valid scene
throw std::runtime_error("expecting empty incoming/outgoing trajectories"); assert(state.scene());
if (!state.scene()) // incoming and outgoing must not contain elements both
throw std::runtime_error("expecting valid planning scene in InterfaceState"); 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 // move state to a list node
std::list<InterfaceState> container; std::list<InterfaceState*> container;
auto it = container.insert(container.end(), std::move(state)); Interface::iterator it = container.insert(container.end(), &state);
assert(it->owner_ == nullptr); // state can only be added once to an interface
it->owner_ = this; it->owner_ = this;
// configure state: inherit priority from other end's state and add current solution's cost // if either incoming or outgoing is defined, derive priority from there
assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set if (!state.incomingTrajectories().empty())
if (incoming) { it->priority_ = InterfaceState::Priority(1, state.incomingTrajectories().front()->cost());
it->priority_ = InterfaceState::Priority(1, incoming->cost()); else if (!state.outgoingTrajectories().empty())
incoming->setEndState(*it); it->priority_ = InterfaceState::Priority(1, state.outgoingTrajectories().front()->cost());
} else if (outgoing) { else // otherwise, assume priority was well defined before
it->priority_ = InterfaceState::Priority(1, outgoing->cost()); assert(it->priority_ >= InterfaceState::Priority(1, 0.0));
outgoing->setStartState(*it);
}
// move list node into interface's state list (sorted by priority) // move list node into interface's state list (sorted by priority)
moveFrom(it, container); moveFrom(it, container);
// and finally call notify callback // and finally call notify callback
if (notify_) notify_(it, false); 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) 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) void Interface::updatePriority(InterfaceState *state, const InterfaceState::Priority& priority)
{ {
if (priority != state->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 // state should be part of the interface
assert(it != end()); assert(it != end());
state->priority_ = priority; state->priority_ = priority;

View File

@ -224,16 +224,6 @@ bool Task::plan()
return numSolutions() > 0; 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) void Task::publishAllSolutions(bool wait)
{ {
enableIntrospection(true); enableIntrospection(true);

View File

@ -148,7 +148,7 @@ QVariant LocalTaskModel::data(const QModelIndex &index, int role) const
case Qt::DisplayRole: case Qt::DisplayRole:
switch (index.column()) { switch (index.column()) {
case 0: return QString::fromStdString(n->name()); 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; case 2: return 0;
} }
break; break;