mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
ae6b86c7d7
commit
c2dd28abae
@ -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;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -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)
|
||||||
|
|
||||||
|
|||||||
@ -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);
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -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>
|
||||||
|
|||||||
@ -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_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -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 ¬ify = NotifyFunction());
|
Interface(const NotifyFunction ¬ify = NotifyFunction());
|
||||||
|
|
||||||
/// add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state
|
/// add a new InterfaceState
|
||||||
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 {
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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 ¤t)
|
|||||||
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 ¤t)
|
|||||||
// 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 ¤t)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 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)
|
||||||
|
|||||||
@ -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();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -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 ¬ify)
|
|||||||
: 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;
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user