mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge #156: Rework interface resolution
This commit is contained in:
commit
1f8dae86dd
@ -91,8 +91,6 @@ public:
|
|||||||
PRIVATE_CLASS(SerialContainer)
|
PRIVATE_CLASS(SerialContainer)
|
||||||
SerialContainer(const std::string& name = "serial container");
|
SerialContainer(const std::string& name = "serial container");
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
|
||||||
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
void compute() override;
|
void compute() override;
|
||||||
|
|
||||||
@ -130,8 +128,6 @@ public:
|
|||||||
PRIVATE_CLASS(ParallelContainerBase)
|
PRIVATE_CLASS(ParallelContainerBase)
|
||||||
ParallelContainerBase(const std::string& name = "parallel container");
|
ParallelContainerBase(const std::string& name = "parallel container");
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||||
|
|
||||||
|
|||||||
@ -105,6 +105,10 @@ public:
|
|||||||
|
|
||||||
void validateConnectivity() const override;
|
void validateConnectivity() const override;
|
||||||
|
|
||||||
|
// Containers derive their required interface from their children
|
||||||
|
// UNKNOWN until resolveInterface was called
|
||||||
|
InterfaceFlags requiredInterface() const override { return required_interface_; }
|
||||||
|
|
||||||
// forward these methods to the public interface for containers
|
// forward these methods to the public interface for containers
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
void compute() override;
|
void compute() override;
|
||||||
@ -115,21 +119,17 @@ public:
|
|||||||
protected:
|
protected:
|
||||||
ContainerBasePrivate(ContainerBase* me, const std::string& name);
|
ContainerBasePrivate(ContainerBase* me, const std::string& name);
|
||||||
|
|
||||||
// Set child's push interfaces: allow pushing if child requires it or
|
// Set child's push interfaces: allow pushing if child requires it.
|
||||||
// if the interface is unknown: in this case greedily assume a push interface.
|
inline void setChildsPushBackwardInterface(StagePrivate* child) {
|
||||||
// If, during pruneInterface() later, we notice that it's not needed, prune there.
|
InterfaceFlags required = child->requiredInterface();
|
||||||
inline void setChildsPushBackwardInterface(Stage& child) {
|
bool allowed = (required & WRITES_PREV_END);
|
||||||
InterfaceFlags required = child.pimpl()->requiredInterface();
|
child->setPrevEnds(allowed ? pending_backward_ : InterfacePtr());
|
||||||
bool allowed = (required & WRITES_PREV_END) || (required == UNKNOWN);
|
|
||||||
child.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr());
|
|
||||||
}
|
}
|
||||||
inline void setChildsPushForwardInterface(Stage& child) {
|
inline void setChildsPushForwardInterface(StagePrivate* child) {
|
||||||
InterfaceFlags required = child.pimpl()->requiredInterface();
|
InterfaceFlags required = child->requiredInterface();
|
||||||
bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN);
|
bool allowed = (required & WRITES_NEXT_START);
|
||||||
child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
||||||
}
|
}
|
||||||
// report error about mismatching interface (start or end as determined by mask)
|
|
||||||
void mismatchingInterface(InitStageException& errors, const StagePrivate& child, const InterfaceFlags mask) const;
|
|
||||||
|
|
||||||
/// 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);
|
||||||
@ -139,6 +139,9 @@ protected:
|
|||||||
auto& internalToExternalMap() { return internal_to_external_; }
|
auto& internalToExternalMap() { return internal_to_external_; }
|
||||||
const auto& internalToExternalMap() const { return internal_to_external_; }
|
const auto& internalToExternalMap() const { return internal_to_external_; }
|
||||||
|
|
||||||
|
// set in resolveInterface()
|
||||||
|
InterfaceFlags required_interface_;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
container_type children_;
|
container_type children_;
|
||||||
|
|
||||||
@ -166,28 +169,20 @@ class SerialContainerPrivate : public ContainerBasePrivate
|
|||||||
public:
|
public:
|
||||||
SerialContainerPrivate(SerialContainer* me, const std::string& name);
|
SerialContainerPrivate(SerialContainer* me, const std::string& name);
|
||||||
|
|
||||||
// containers derive their required interface from their children
|
|
||||||
InterfaceFlags requiredInterface() const override;
|
|
||||||
|
|
||||||
// called by parent asking for pruning of this' interface
|
// called by parent asking for pruning of this' interface
|
||||||
void pruneInterface(InterfaceFlags accepted) override;
|
void resolveInterface(InterfaceFlags expected) override;
|
||||||
// validate connectivity of chain
|
// validate connectivity of chain
|
||||||
void validateConnectivity() const override;
|
void validateConnectivity() const override;
|
||||||
|
|
||||||
private:
|
void reset();
|
||||||
// connect cur stage to its predecessor and successor
|
|
||||||
bool connect(container_type::const_iterator cur);
|
|
||||||
|
|
||||||
// called by init() to prune interfaces for children in range [first, last)
|
protected:
|
||||||
void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end);
|
// connect two neighbors
|
||||||
// prune interface for children in range [first, last) to given direction
|
void connect(StagePrivate& stage1, StagePrivate& stage2);
|
||||||
void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end,
|
|
||||||
InterfaceFlags accepted);
|
|
||||||
// store first/last child's interface as required for this container
|
|
||||||
void storeRequiredInterface(container_type::const_iterator first, container_type::const_iterator end);
|
|
||||||
|
|
||||||
private:
|
// validate that child's interface matches mine (considering start or end only as determined by mask)
|
||||||
InterfaceFlags required_interface_;
|
template <unsigned int mask>
|
||||||
|
void validateInterface(const StagePrivate& child, InterfaceFlags required) const;
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(SerialContainer)
|
PIMPL_FUNCTIONS(SerialContainer)
|
||||||
|
|
||||||
@ -219,14 +214,14 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
|
|||||||
public:
|
public:
|
||||||
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name);
|
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name);
|
||||||
|
|
||||||
// containers derive their required interface from their children
|
|
||||||
InterfaceFlags requiredInterface() const override;
|
|
||||||
|
|
||||||
// called by parent asking for pruning of this' interface
|
// called by parent asking for pruning of this' interface
|
||||||
void pruneInterface(InterfaceFlags accepted) override;
|
void resolveInterface(InterfaceFlags expected) override;
|
||||||
|
|
||||||
void validateConnectivity() const override;
|
void validateConnectivity() const override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
|
||||||
|
|
||||||
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);
|
||||||
@ -256,7 +251,7 @@ public:
|
|||||||
typedef std::function<void(SubTrajectory&&)> Spawner;
|
typedef std::function<void(SubTrajectory&&)> Spawner;
|
||||||
MergerPrivate(Merger* me, const std::string& name);
|
MergerPrivate(Merger* me, const std::string& name);
|
||||||
|
|
||||||
InterfaceFlags requiredInterface() const override;
|
void resolveInterface(InterfaceFlags expected) override;
|
||||||
|
|
||||||
void onNewPropagateSolution(const SolutionBase& s);
|
void onNewPropagateSolution(const SolutionBase& s);
|
||||||
void onNewGeneratorSolution(const SolutionBase& s);
|
void onNewGeneratorSolution(const SolutionBase& s);
|
||||||
|
|||||||
@ -83,13 +83,30 @@ enum InterfaceFlag
|
|||||||
PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END,
|
PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END,
|
||||||
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
|
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef Flags<InterfaceFlag> InterfaceFlags;
|
typedef Flags<InterfaceFlag> InterfaceFlags;
|
||||||
|
|
||||||
|
/** invert interface such that
|
||||||
|
* - new end can connect to old start
|
||||||
|
* - new start can connect to old end
|
||||||
|
*/
|
||||||
|
constexpr InterfaceFlags invert(InterfaceFlags f) {
|
||||||
|
InterfaceFlags inv;
|
||||||
|
if (f & READS_START)
|
||||||
|
inv |= WRITES_NEXT_START;
|
||||||
|
if (f & WRITES_PREV_END)
|
||||||
|
inv |= READS_END;
|
||||||
|
if (f & READS_END)
|
||||||
|
inv |= WRITES_PREV_END;
|
||||||
|
if (f & WRITES_NEXT_START)
|
||||||
|
inv |= READS_START;
|
||||||
|
return inv;
|
||||||
|
};
|
||||||
|
|
||||||
// some useful constants
|
// some useful constants
|
||||||
constexpr InterfaceFlags UNKNOWN;
|
constexpr InterfaceFlags UNKNOWN;
|
||||||
constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
|
constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
|
||||||
constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START });
|
constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START });
|
||||||
constexpr InterfaceFlags PROPAGATE_BOTHWAYS({ PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS });
|
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(Interface)
|
MOVEIT_CLASS_FORWARD(Interface)
|
||||||
MOVEIT_CLASS_FORWARD(Stage)
|
MOVEIT_CLASS_FORWARD(Stage)
|
||||||
@ -253,12 +270,9 @@ public:
|
|||||||
AUTO = 0x00, // auto-derive direction from context
|
AUTO = 0x00, // auto-derive direction from context
|
||||||
FORWARD = 0x01, // propagate forward only
|
FORWARD = 0x01, // propagate forward only
|
||||||
BACKWARD = 0x02, // propagate backward only
|
BACKWARD = 0x02, // propagate backward only
|
||||||
BOTHWAY = FORWARD | BACKWARD, // propagate both ways
|
|
||||||
};
|
};
|
||||||
void restrictDirection(Direction dir);
|
void restrictDirection(Direction dir);
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
|
||||||
|
|
||||||
virtual void computeForward(const InterfaceState& from) = 0;
|
virtual void computeForward(const InterfaceState& from) = 0;
|
||||||
virtual void computeBackward(const InterfaceState& to) = 0;
|
virtual void computeBackward(const InterfaceState& to) = 0;
|
||||||
|
|
||||||
@ -368,6 +382,8 @@ protected:
|
|||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
const char* flowSymbol(moveit::task_constructor::InterfaceFlags f);
|
/** Return (horizontal) flow symbol for start or end interface (specified by mask) */
|
||||||
|
template <unsigned int mask>
|
||||||
|
const char* flowSymbol(InterfaceFlags f);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -71,15 +71,11 @@ public:
|
|||||||
/// actually configured interface of this stage (only valid after init())
|
/// actually configured interface of this stage (only valid after init())
|
||||||
InterfaceFlags interfaceFlags() const;
|
InterfaceFlags interfaceFlags() const;
|
||||||
|
|
||||||
/** Interface required by this stage
|
/// Retrieve interface required by this stage, UNKNOWN if auto-detected from context
|
||||||
*
|
|
||||||
* If the interface is unknown (because it is auto-detected from context), return 0 */
|
|
||||||
virtual InterfaceFlags requiredInterface() const = 0;
|
virtual InterfaceFlags requiredInterface() const = 0;
|
||||||
|
|
||||||
/** Prune interface to comply with the given propagation direction
|
/// Resolve interface/propagation direction to comply with the given external interface
|
||||||
*
|
virtual void resolveInterface(InterfaceFlags /* expected */) {}
|
||||||
* PropagatingEitherWay uses this in restrictDirection() */
|
|
||||||
virtual void pruneInterface(InterfaceFlags accepted) {}
|
|
||||||
|
|
||||||
/// validate connectivity of children (after init() was done)
|
/// validate connectivity of children (after init() was done)
|
||||||
virtual void validateConnectivity() const;
|
virtual void validateConnectivity() const;
|
||||||
@ -163,6 +159,7 @@ protected:
|
|||||||
std::string name_;
|
std::string name_;
|
||||||
PropertyMap properties_;
|
PropertyMap properties_;
|
||||||
|
|
||||||
|
// pull interfaces, created by the stage as required
|
||||||
InterfacePtr starts_;
|
InterfacePtr starts_;
|
||||||
InterfacePtr ends_;
|
InterfacePtr ends_;
|
||||||
|
|
||||||
@ -182,6 +179,8 @@ private:
|
|||||||
ContainerBase* parent_; // owning parent
|
ContainerBase* parent_; // owning parent
|
||||||
container_type::iterator it_; // iterator into parent's children_ list referring to this
|
container_type::iterator it_; // iterator into parent's children_ list referring to this
|
||||||
|
|
||||||
|
// push interfaces, assigned by the parent container
|
||||||
|
// linking to previous/next sibling's pull interfaces
|
||||||
InterfaceWeakPtr prev_ends_; // interface to be used for sendBackward()
|
InterfaceWeakPtr prev_ends_; // interface to be used for sendBackward()
|
||||||
InterfaceWeakPtr next_starts_; // interface to be used for sendForward()
|
InterfaceWeakPtr next_starts_; // interface to be used for sendForward()
|
||||||
|
|
||||||
@ -208,19 +207,17 @@ class PropagatingEitherWayPrivate : public ComputeBasePrivate
|
|||||||
friend class PropagatingEitherWay;
|
friend class PropagatingEitherWay;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
PropagatingEitherWay::Direction required_interface_dirs_;
|
PropagatingEitherWay::Direction configured_dir_;
|
||||||
|
InterfaceFlags required_interface_;
|
||||||
|
|
||||||
inline PropagatingEitherWayPrivate(PropagatingEitherWay* me,
|
inline PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction configured_dir_,
|
||||||
PropagatingEitherWay::Direction required_interface_dirs_,
|
|
||||||
const std::string& name);
|
const std::string& name);
|
||||||
|
|
||||||
InterfaceFlags requiredInterface() const override;
|
InterfaceFlags requiredInterface() const override;
|
||||||
// initialize pull interfaces for given propagation directions
|
// initialize pull interfaces for given propagation directions
|
||||||
void initInterface(PropagatingEitherWay::Direction dir);
|
void initInterface(PropagatingEitherWay::Direction dir);
|
||||||
// prune interface to the given propagation direction
|
// resolve interface to the given propagation direction
|
||||||
void pruneInterface(InterfaceFlags accepted) override;
|
void resolveInterface(InterfaceFlags expected) override;
|
||||||
// validate that we can propagate in one direction at least
|
|
||||||
void validateConnectivity() const override;
|
|
||||||
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
void compute() override;
|
void compute() override;
|
||||||
|
|||||||
@ -53,10 +53,11 @@ using namespace std::placeholders;
|
|||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
|
|
||||||
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name) : StagePrivate(me, name) {
|
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
|
||||||
pending_backward_.reset(new Interface);
|
: StagePrivate(me, name)
|
||||||
pending_forward_.reset(new Interface);
|
, required_interface_(UNKNOWN)
|
||||||
}
|
, pending_backward_(new Interface)
|
||||||
|
, pending_forward_(new Interface) {}
|
||||||
|
|
||||||
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
|
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
|
||||||
if (!for_insert && index < 0)
|
if (!for_insert && index < 0)
|
||||||
@ -90,25 +91,9 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback& pr
|
|||||||
}
|
}
|
||||||
|
|
||||||
void ContainerBasePrivate::validateConnectivity() const {
|
void ContainerBasePrivate::validateConnectivity() const {
|
||||||
InitStageException errors;
|
|
||||||
// recursively validate all children and accumulate errors
|
// recursively validate all children and accumulate errors
|
||||||
for (const auto& child : children()) {
|
for (const auto& child : children())
|
||||||
try {
|
child->pimpl()->validateConnectivity();
|
||||||
child->pimpl()->validateConnectivity();
|
|
||||||
} catch (InitStageException& e) {
|
|
||||||
errors.append(e);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (errors)
|
|
||||||
throw errors;
|
|
||||||
}
|
|
||||||
|
|
||||||
void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child,
|
|
||||||
const InterfaceFlags mask) const {
|
|
||||||
boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)");
|
|
||||||
errors.push_back(*me(), (desc % (mask == START_IF_MASK ? "start" : "end") % child.name() %
|
|
||||||
flowSymbol(child.interfaceFlags() & mask) % flowSymbol(interfaceFlags() & mask))
|
|
||||||
.str());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ContainerBasePrivate::canCompute() const {
|
bool ContainerBasePrivate::canCompute() const {
|
||||||
@ -252,6 +237,11 @@ void ContainerBase::reset() {
|
|||||||
// ... and state mapping
|
// ... and state mapping
|
||||||
impl->internal_to_external_.clear();
|
impl->internal_to_external_.clear();
|
||||||
|
|
||||||
|
// interfaces depend on children which might change
|
||||||
|
impl->required_interface_ = UNKNOWN;
|
||||||
|
impl->starts_.reset();
|
||||||
|
impl->ends_.reset();
|
||||||
|
|
||||||
Stage::reset();
|
Stage::reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -401,238 +391,103 @@ SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new
|
|||||||
SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name)
|
SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name)
|
||||||
: ContainerBasePrivate(me, name) {}
|
: ContainerBasePrivate(me, name) {}
|
||||||
|
|
||||||
// a serial container's required interface is derived from the required input interfaces
|
void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) {
|
||||||
// of the first and last children. After resolving, it is remembered in required_interface_.
|
InterfaceFlags flags1 = stage1.requiredInterface();
|
||||||
InterfaceFlags SerialContainerPrivate::requiredInterface() const {
|
InterfaceFlags flags2 = stage2.requiredInterface();
|
||||||
if ((required_interface_ & START_IF_MASK) && (required_interface_ & END_IF_MASK))
|
|
||||||
return required_interface_;
|
|
||||||
|
|
||||||
if (children().empty())
|
if ((flags1 & WRITES_NEXT_START) && (flags2 & READS_START))
|
||||||
return UNKNOWN;
|
stage1.setNextStarts(stage2.starts());
|
||||||
return (children().front()->pimpl()->requiredInterface() & START_IF_MASK) |
|
else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END))
|
||||||
(children().back()->pimpl()->requiredInterface() & END_IF_MASK);
|
stage2.setPrevEnds(stage1.ends());
|
||||||
}
|
else {
|
||||||
|
boost::format desc("cannot connect end interface of '%1%' (%2%) to start interface of '%3%' (%4%)");
|
||||||
// connect cur stage to its predecessor and successor by setting the push interface pointers
|
desc % stage1.name() % flowSymbol<END_IF_MASK>(flags1);
|
||||||
// return true if cur stage should be scheduled for a second sweep
|
desc % stage2.name() % flowSymbol<START_IF_MASK>(flags2);
|
||||||
bool SerialContainerPrivate::connect(container_type::const_iterator cur) {
|
throw InitStageException(*me(), desc.str());
|
||||||
StagePrivate* const cur_impl = **cur;
|
|
||||||
InterfaceFlags required = cur_impl->requiredInterface();
|
|
||||||
|
|
||||||
// get iterators to prev / next stage in sequence
|
|
||||||
auto prev = cur;
|
|
||||||
--prev;
|
|
||||||
auto next = cur;
|
|
||||||
++next;
|
|
||||||
|
|
||||||
// set push forward connection using next's starts
|
|
||||||
if ((required == UNKNOWN || required & WRITES_NEXT_START) &&
|
|
||||||
next != children().end()) // last child has not a next one
|
|
||||||
cur_impl->setNextStarts((*next)->pimpl()->starts());
|
|
||||||
|
|
||||||
// set push backward connection using prev's ends
|
|
||||||
if ((required == UNKNOWN || required & WRITES_PREV_END) &&
|
|
||||||
cur != children().begin()) // first child has not a previous one
|
|
||||||
cur_impl->setPrevEnds((*prev)->pimpl()->ends());
|
|
||||||
|
|
||||||
// schedule stage with unknown interface for 2nd sweep
|
|
||||||
return required == UNKNOWN || required == PROPAGATE_BOTHWAYS;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Establishing the interface connections, we face a chicken-egg-problem:
|
|
||||||
* To establish a connection, a predecessors/successors pull interface is
|
|
||||||
* assigned to the current's stage push interface.
|
|
||||||
* However, propagating stages (in auto-detection mode) can only create
|
|
||||||
* their pull interfaces if the corresponding, opposite-side push interface
|
|
||||||
* is present already (because that's the mechanism to determine the supported
|
|
||||||
* propagation directions).
|
|
||||||
*
|
|
||||||
* Hence, we need to resolve this by performing two sweeps:
|
|
||||||
* - initialization, assuming both propagation directions should be supported,
|
|
||||||
* thus generating both pull interfaces, i.e. providing the egg
|
|
||||||
* - stripping down the interfaces to the actual context
|
|
||||||
* This context is provided by two stages pushing from both ends
|
|
||||||
* into a (potentially long) sequence of propagating stages (tbd).
|
|
||||||
*/
|
|
||||||
void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
|
||||||
// reset pull interfaces
|
|
||||||
auto impl = pimpl();
|
|
||||||
impl->starts_.reset();
|
|
||||||
impl->ends_.reset();
|
|
||||||
impl->required_interface_ = UNKNOWN;
|
|
||||||
|
|
||||||
// recursively init all children, throws if there are no children
|
|
||||||
ContainerBase::init(robot_model);
|
|
||||||
|
|
||||||
auto start = impl->children().begin();
|
|
||||||
auto last = --impl->children().end();
|
|
||||||
|
|
||||||
// connect first / last child's push interfaces to our pending_* buffers
|
|
||||||
// if they require pushing
|
|
||||||
impl->setChildsPushBackwardInterface(**start);
|
|
||||||
impl->setChildsPushForwardInterface(**last);
|
|
||||||
|
|
||||||
// initialize and connect remaining children in two sweeps
|
|
||||||
// to allow interface auto-detection for propagating stages
|
|
||||||
auto first_unknown = start; // pointer to first stage with unknown interface
|
|
||||||
for (auto cur = start, end = impl->children().end(); cur != end; ++cur) {
|
|
||||||
// 1st sweep: connect everything potentially possible,
|
|
||||||
// remembering start of unknown sub sequence
|
|
||||||
if (impl->connect(cur))
|
|
||||||
;
|
|
||||||
else { // reached a stage with known interface
|
|
||||||
// 2nd sweep: prune interfaces from [first_unknown, cur)
|
|
||||||
impl->pruneInterfaces(first_unknown, cur);
|
|
||||||
// restart with first_unknown = ++cur
|
|
||||||
first_unknown = cur;
|
|
||||||
++first_unknown;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
// prune stages [first_unknown, end())
|
|
||||||
impl->pruneInterfaces(first_unknown, impl->children().end());
|
|
||||||
|
|
||||||
// initialize this' pull interfaces if first/last child pulls
|
|
||||||
if (const InterfacePtr& target = (*start)->pimpl()->starts())
|
|
||||||
impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, std::placeholders::_1,
|
|
||||||
std::cref(target), std::placeholders::_2)));
|
|
||||||
if (const InterfacePtr& target = (*last)->pimpl()->ends())
|
|
||||||
impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, std::placeholders::_1,
|
|
||||||
std::cref(target), std::placeholders::_2)));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// prune interface for children in range [first, last) to given direction
|
template <unsigned int mask>
|
||||||
void SerialContainerPrivate::storeRequiredInterface(container_type::const_iterator first,
|
void SerialContainerPrivate::validateInterface(const StagePrivate& child, InterfaceFlags required) const {
|
||||||
container_type::const_iterator end) {
|
required = required & mask;
|
||||||
if (first == children().begin())
|
if (required == UNKNOWN)
|
||||||
required_interface_ |= children().front()->pimpl()->interfaceFlags() & START_IF_MASK;
|
return; // cannot yet validate
|
||||||
if (end == children().end() && !children().empty())
|
InterfaceFlags child_interface = child.interfaceFlags() & mask;
|
||||||
required_interface_ |= children().back()->pimpl()->interfaceFlags() & END_IF_MASK;
|
if (required != child_interface) {
|
||||||
|
boost::format desc("%1% interface (%3%) of '%2%' does not match mine (%4%)");
|
||||||
|
desc % (mask == START_IF_MASK ? "start" : "end") % child.name();
|
||||||
|
desc % flowSymbol<mask>(child_interface) % flowSymbol<mask>(required);
|
||||||
|
throw InitStageException(*me_, desc.str());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// called by parent asking for pruning of this' interface
|
// called by parent asking for pruning of this' interface
|
||||||
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
|
||||||
|
// we need to have some children to do the actual work
|
||||||
if (children().empty())
|
if (children().empty())
|
||||||
return;
|
throw InitStageException(*me(), "no children");
|
||||||
|
|
||||||
// reading is always allowed if current interface flags do so
|
if (!(expected & START_IF_MASK))
|
||||||
accepted |= (interfaceFlags() & InterfaceFlags({ READS_START, READS_END }));
|
throw InitStageException(*me(), "unknown start interface");
|
||||||
|
|
||||||
if (accepted == PROPAGATE_BOTHWAYS)
|
Stage& first = *children().front();
|
||||||
return; // There is nothing to prune
|
Stage& last = *children().back();
|
||||||
|
|
||||||
// If whole chain is still undecided, prune all children
|
InitStageException exceptions;
|
||||||
if (children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS &&
|
|
||||||
children().back()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) {
|
try { // FIRST child
|
||||||
pruneInterfaces(children().begin(), children().end(), accepted);
|
first.pimpl()->resolveInterface(expected & START_IF_MASK);
|
||||||
} else { // otherwise only prune the first / last child's input / output interface
|
// connect first child's (start) push interface
|
||||||
StagePrivate* child_impl;
|
setChildsPushBackwardInterface(first.pimpl());
|
||||||
child_impl = children().front()->pimpl();
|
// validate that first child's and this container's start interfaces match
|
||||||
child_impl->pruneInterface((accepted & START_IF_MASK) | (child_impl->interfaceFlags() & END_IF_MASK));
|
validateInterface<START_IF_MASK>(*first.pimpl(), expected);
|
||||||
child_impl = children().back()->pimpl();
|
// connect first child's (start) pull interface
|
||||||
child_impl->pruneInterface((accepted & END_IF_MASK) | (child_impl->interfaceFlags() & START_IF_MASK));
|
if (const InterfacePtr& target = first.pimpl()->starts())
|
||||||
|
starts_.reset(new Interface(
|
||||||
|
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
|
||||||
|
} catch (InitStageException& e) {
|
||||||
|
exceptions.append(e);
|
||||||
}
|
}
|
||||||
|
|
||||||
// reset my pull interfaces, if first/last child don't pull anymore
|
// process all children and connect them
|
||||||
if (!children().front()->pimpl()->starts())
|
for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) {
|
||||||
starts_.reset();
|
try {
|
||||||
if (!children().back()->pimpl()->ends())
|
StagePrivate* child_impl = (**it).pimpl();
|
||||||
ends_.reset();
|
StagePrivate* previous_impl = (**previous_it).pimpl();
|
||||||
|
child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
|
||||||
if (interfaceFlags() == UNKNOWN)
|
connect(*previous_impl, *child_impl);
|
||||||
throw InitStageException(*me(), "failed to derive propagation direction");
|
} catch (InitStageException& e) {
|
||||||
}
|
exceptions.append(e);
|
||||||
|
}
|
||||||
// called by init() to prune interfaces for children in range [first, last)
|
|
||||||
// this function determines the feasible propagation directions
|
|
||||||
void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end) {
|
|
||||||
if (first == end) {
|
|
||||||
storeRequiredInterface(first, end);
|
|
||||||
return; // nothing to do in this case
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// determine accepted interface from available push interfaces
|
try { // connect last child's (end) push interface
|
||||||
InterfaceFlags accepted;
|
setChildsPushForwardInterface(last.pimpl());
|
||||||
|
// validate that last child's and this container's end interfaces match
|
||||||
// if first stage ...
|
validateInterface<END_IF_MASK>(*last.pimpl(), expected);
|
||||||
if (first != children().begin()) {
|
// connect last child's (end) pull interface
|
||||||
auto prev = first;
|
if (const InterfacePtr& target = last.pimpl()->ends())
|
||||||
--prev; // pointer to previous stage
|
ends_.reset(new Interface(
|
||||||
// ... pushes forward, we accept forward propagation
|
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
|
||||||
if ((*prev)->pimpl()->requiredInterface() & WRITES_NEXT_START)
|
} catch (InitStageException& e) {
|
||||||
accepted |= PROPAGATE_FORWARDS;
|
exceptions.append(e);
|
||||||
// ... pulls backward, we accept backward propagation
|
|
||||||
if ((*prev)->pimpl()->requiredInterface() & READS_END)
|
|
||||||
accepted |= PROPAGATE_BACKWARDS;
|
|
||||||
} // else: for first child we cannot determine the interface yet
|
|
||||||
|
|
||||||
// if end stage ...
|
|
||||||
if (end != children().end()) {
|
|
||||||
// ... pushes backward, we accept backward propagation
|
|
||||||
if ((*end)->pimpl()->requiredInterface() & WRITES_PREV_END)
|
|
||||||
accepted |= PROPAGATE_BACKWARDS;
|
|
||||||
// ... pulls forward, we accept forward propagation
|
|
||||||
if ((*end)->pimpl()->requiredInterface() & READS_START)
|
|
||||||
accepted |= PROPAGATE_FORWARDS;
|
|
||||||
} // else: for last child we cannot determine the interface yet
|
|
||||||
|
|
||||||
// nothing to do if:
|
|
||||||
// - accepted == UNKNOWN: interface still unknown
|
|
||||||
// - accepted == PROPAGATE_BOTHWAYS: no change
|
|
||||||
if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS)
|
|
||||||
pruneInterfaces(first, end, accepted);
|
|
||||||
}
|
|
||||||
|
|
||||||
// prune interface for children in range [first, last) to given direction
|
|
||||||
void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end,
|
|
||||||
InterfaceFlags accepted) {
|
|
||||||
// 1st sweep: remove push interfaces
|
|
||||||
for (auto it = first; it != end; ++it) {
|
|
||||||
StagePrivate* impl = (*it)->pimpl();
|
|
||||||
// the required interface should be a subset of the accepted one
|
|
||||||
if ((impl->requiredInterface() & accepted) != impl->requiredInterface())
|
|
||||||
throw InitStageException(*impl->me(), "Required interface not satisfied after pruning");
|
|
||||||
|
|
||||||
// remove push interfaces if not accepted
|
|
||||||
if (!(accepted & WRITES_PREV_END))
|
|
||||||
impl->setPrevEnds(InterfacePtr());
|
|
||||||
|
|
||||||
if (!(accepted & WRITES_NEXT_START))
|
|
||||||
impl->setNextStarts(InterfacePtr());
|
|
||||||
}
|
|
||||||
// 2nd sweep: recursively prune children
|
|
||||||
for (auto it = first; it != end; ++it) {
|
|
||||||
StagePrivate* impl = (*it)->pimpl();
|
|
||||||
impl->pruneInterface(accepted);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
storeRequiredInterface(first, end);
|
required_interface_ = first.pimpl()->interfaceFlags() & START_IF_MASK | last.pimpl()->interfaceFlags() & END_IF_MASK;
|
||||||
|
|
||||||
|
if (exceptions)
|
||||||
|
throw exceptions;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SerialContainerPrivate::validateConnectivity() const {
|
void SerialContainerPrivate::validateConnectivity() const {
|
||||||
InitStageException errors;
|
ContainerBasePrivate::validateConnectivity();
|
||||||
|
|
||||||
// recursively validate children
|
|
||||||
try {
|
|
||||||
ContainerBasePrivate::validateConnectivity();
|
|
||||||
} catch (InitStageException& e) {
|
|
||||||
errors.append(e);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
InterfaceFlags mine = interfaceFlags();
|
||||||
// check that input / output interface of first / last child matches this' resp. interface
|
// check that input / output interface of first / last child matches this' resp. interface
|
||||||
if (!children().empty()) {
|
validateInterface<START_IF_MASK>(*children().front()->pimpl(), mine);
|
||||||
const StagePrivate* start = children().front()->pimpl();
|
validateInterface<END_IF_MASK>(*children().back()->pimpl(), mine);
|
||||||
const auto my_flags = this->interfaceFlags();
|
|
||||||
auto child_flags = start->interfaceFlags() & START_IF_MASK;
|
|
||||||
if (child_flags != (my_flags & START_IF_MASK))
|
|
||||||
mismatchingInterface(errors, *start, START_IF_MASK);
|
|
||||||
|
|
||||||
const StagePrivate* last = children().back()->pimpl();
|
// validate connectivity of children between each other
|
||||||
child_flags = last->interfaceFlags() & END_IF_MASK;
|
|
||||||
if (child_flags != (my_flags & END_IF_MASK))
|
|
||||||
mismatchingInterface(errors, *last, END_IF_MASK);
|
|
||||||
}
|
|
||||||
|
|
||||||
// validate connectivity of children amongst each other
|
|
||||||
// ContainerBasePrivate::validateConnectivity() ensures that required push interfaces are present,
|
// ContainerBasePrivate::validateConnectivity() ensures that required push interfaces are present,
|
||||||
// that is, neighbouring stages have a corresponding pull interface.
|
// that is, neighbouring stages have a corresponding pull interface.
|
||||||
// Here, it remains to check that - if a child has a pull interface - it's indeed feeded.
|
// Here, it remains to check that - if a child has a pull interface - it's indeed feeded.
|
||||||
@ -649,16 +504,13 @@ void SerialContainerPrivate::validateConnectivity() const {
|
|||||||
// start pull interface fed?
|
// start pull interface fed?
|
||||||
if (cur != children().begin() && // first child has not a previous one
|
if (cur != children().begin() && // first child has not a previous one
|
||||||
(required & READS_START) && !(*prev)->pimpl()->nextStarts())
|
(required & READS_START) && !(*prev)->pimpl()->nextStarts())
|
||||||
errors.push_back(**cur, "start interface is not fed");
|
throw InitStageException(**cur, "start interface is not fed");
|
||||||
|
|
||||||
// end pull interface fed?
|
// end pull interface fed?
|
||||||
if (next != end && // last child has not a next one
|
if (next != end && // last child has not a next one
|
||||||
(required & READS_END) && !(*next)->pimpl()->prevEnds())
|
(required & READS_END) && !(*next)->pimpl()->prevEnds())
|
||||||
errors.push_back(**cur, "end interface is not fed");
|
throw InitStageException(**cur, "end interface is not fed");
|
||||||
}
|
}
|
||||||
|
|
||||||
if (errors)
|
|
||||||
throw errors;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SerialContainer::canCompute() const {
|
bool SerialContainer::canCompute() const {
|
||||||
@ -715,74 +567,74 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti
|
|||||||
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
|
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
|
||||||
: ContainerBasePrivate(me, name) {}
|
: ContainerBasePrivate(me, name) {}
|
||||||
|
|
||||||
// A parallel container's required interface is derived from the required interfaces of all of its children.
|
void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
|
||||||
// They must not conflict to each other. Otherwise an InitStageException is thrown.
|
// we need to have some children to do the actual work
|
||||||
InterfaceFlags ParallelContainerBasePrivate::requiredInterface() const {
|
|
||||||
if (children().empty())
|
if (children().empty())
|
||||||
return UNKNOWN;
|
throw InitStageException(*me(), "no children");
|
||||||
/* The interfaces of all children need to be consistent with each other. Allowed combinations are:
|
|
||||||
* ❘ ❘ = ❘ (connecting stages)
|
|
||||||
* ↑ ↑ = ↑ (backward propagating)
|
|
||||||
* ↓ ↓ = ↓ (forward propagating)
|
|
||||||
* ↑ ↓ = ⇅ = ⇅ ↑ = ⇅ ↓ (propagating in both directions)
|
|
||||||
* ↕ ↕ = ↕ (generating)
|
|
||||||
*/
|
|
||||||
|
|
||||||
InterfaceFlags accumulated = children().front()->pimpl()->requiredInterface();
|
InitStageException exceptions;
|
||||||
for (const Stage::pointer& stage : children()) {
|
|
||||||
InterfaceFlags current = stage->pimpl()->requiredInterface();
|
bool first = true;
|
||||||
if (accumulated != PROPAGATE_BOTHWAYS &&
|
for (const Stage::pointer& child : children()) {
|
||||||
(accumulated & current) == current) // all flags of current are already available in accumulated
|
try {
|
||||||
|
auto child_impl = child->pimpl();
|
||||||
|
child_impl->resolveInterface(expected);
|
||||||
|
validateInterfaces(*child_impl, expected, first);
|
||||||
|
// initialize push connections of children according to their demands
|
||||||
|
setChildsPushForwardInterface(child_impl);
|
||||||
|
setChildsPushBackwardInterface(child_impl);
|
||||||
|
first = false;
|
||||||
|
} catch (InitStageException& e) {
|
||||||
|
exceptions.append(e);
|
||||||
continue;
|
continue;
|
||||||
|
}
|
||||||
bool current_is_propagating =
|
|
||||||
(current == PROPAGATE_BOTHWAYS || current == PROPAGATE_FORWARDS || current == PROPAGATE_BACKWARDS);
|
|
||||||
|
|
||||||
if (current_is_propagating && accumulated != CONNECT && accumulated != GENERATE)
|
|
||||||
accumulated |= current; // propagating is compatible to all except CONNECT and GENERATE
|
|
||||||
else
|
|
||||||
throw InitStageException(*me(),
|
|
||||||
"child '" + stage->name() + "' has conflicting interface to previous children");
|
|
||||||
}
|
}
|
||||||
return accumulated;
|
|
||||||
|
if (exceptions)
|
||||||
|
throw exceptions;
|
||||||
|
|
||||||
|
// States received by the container need to be copied to all children's pull interfaces.
|
||||||
|
if (expected & READS_START)
|
||||||
|
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
|
||||||
|
this->onNewExternalState(Interface::FORWARD, external, updated);
|
||||||
|
}));
|
||||||
|
if (expected & READS_END)
|
||||||
|
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
|
||||||
|
this->onNewExternalState(Interface::BACKWARD, external, updated);
|
||||||
|
}));
|
||||||
|
|
||||||
|
required_interface_ = expected;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
|
void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
|
||||||
// forward pruning to all children with UNKNOWN required interface
|
bool first) const {
|
||||||
for (const Stage::pointer& stage : children()) {
|
const InterfaceFlags child_interface = child.requiredInterface();
|
||||||
if (stage->pimpl()->requiredInterface() == UNKNOWN)
|
bool valid = true;
|
||||||
stage->pimpl()->pruneInterface(accepted);
|
for (InterfaceFlags mask : { START_IF_MASK, END_IF_MASK }) {
|
||||||
|
if ((external & mask) == UNKNOWN)
|
||||||
|
external |= child_interface & mask;
|
||||||
|
|
||||||
|
valid = valid & ((external & mask) == (child_interface & mask));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!valid) {
|
||||||
|
boost::format desc("interface of '%1%' (%3% %4%) does not match %2% (%5% %6%).");
|
||||||
|
desc % child.name();
|
||||||
|
desc % (first ? "external one" : "other children's");
|
||||||
|
desc % flowSymbol<START_IF_MASK>(child_interface) % flowSymbol<END_IF_MASK>(child_interface);
|
||||||
|
desc % flowSymbol<START_IF_MASK>(external) % flowSymbol<END_IF_MASK>(external);
|
||||||
|
throw InitStageException(*me_, desc.str());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParallelContainerBasePrivate::validateConnectivity() const {
|
void ParallelContainerBasePrivate::validateConnectivity() const {
|
||||||
InitStageException errors;
|
|
||||||
InterfaceFlags my_interface = interfaceFlags();
|
InterfaceFlags my_interface = interfaceFlags();
|
||||||
InterfaceFlags children_interfaces;
|
|
||||||
|
|
||||||
// check that input / output interfaces of all children are handled by my interface
|
// check that input / output interfaces of all children are handled by my interface
|
||||||
for (const auto& child : children()) {
|
for (const auto& child : children())
|
||||||
InterfaceFlags current = child->pimpl()->interfaceFlags();
|
validateInterfaces(*child->pimpl(), my_interface);
|
||||||
children_interfaces |= current; // compute union of all children interfaces
|
|
||||||
|
|
||||||
if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK))
|
ContainerBasePrivate::validateConnectivity();
|
||||||
mismatchingInterface(errors, *child->pimpl(), START_IF_MASK);
|
|
||||||
if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK))
|
|
||||||
mismatchingInterface(errors, *child->pimpl(), END_IF_MASK);
|
|
||||||
}
|
|
||||||
// check that there is a child matching the expected push interfaces
|
|
||||||
if ((my_interface & GENERATE) != (children_interfaces & GENERATE))
|
|
||||||
errors.push_back(*me(), "no child provides expected push interface");
|
|
||||||
|
|
||||||
// recursively validate children
|
|
||||||
try {
|
|
||||||
ContainerBasePrivate::validateConnectivity();
|
|
||||||
} catch (InitStageException& e) {
|
|
||||||
errors.append(e);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (errors)
|
|
||||||
throw errors;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external,
|
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external,
|
||||||
@ -795,35 +647,6 @@ ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl)
|
|||||||
ParallelContainerBase::ParallelContainerBase(const std::string& name)
|
ParallelContainerBase::ParallelContainerBase(const std::string& name)
|
||||||
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
|
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
|
||||||
|
|
||||||
/* 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.
|
|
||||||
*/
|
|
||||||
void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
|
||||||
// recursively init children
|
|
||||||
ContainerBase::init(robot_model);
|
|
||||||
auto impl = pimpl();
|
|
||||||
|
|
||||||
// determine the union of interfaces required by children
|
|
||||||
// TODO: should we better use the least common interface?
|
|
||||||
InterfaceFlags required = impl->requiredInterface();
|
|
||||||
|
|
||||||
// initialize this' pull connections
|
|
||||||
impl->starts().reset(required & READS_START ?
|
|
||||||
new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl,
|
|
||||||
Interface::FORWARD, std::placeholders::_1, std::placeholders::_2)) :
|
|
||||||
nullptr);
|
|
||||||
impl->ends().reset(required & READS_END ?
|
|
||||||
new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl,
|
|
||||||
Interface::BACKWARD, std::placeholders::_1, std::placeholders::_2)) :
|
|
||||||
nullptr);
|
|
||||||
|
|
||||||
// initialize push connections of children according to their demands
|
|
||||||
for (const Stage::pointer& stage : impl->children()) {
|
|
||||||
impl->setChildsPushForwardInterface(*stage);
|
|
||||||
impl->setChildsPushBackwardInterface(*stage);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
|
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
impl->liftSolution(std::make_shared<WrappedSolution>(impl, &solution, cost, std::move(comment)), solution.start(),
|
impl->liftSolution(std::make_shared<WrappedSolution>(impl, &solution, cost, std::move(comment)), solution.start(),
|
||||||
@ -938,24 +761,14 @@ void Fallbacks::onNewSolution(const SolutionBase& s) {
|
|||||||
|
|
||||||
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
|
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
|
||||||
|
|
||||||
InterfaceFlags MergerPrivate::requiredInterface() const {
|
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
|
||||||
if (children().size() < 2)
|
ContainerBasePrivate::resolveInterface(expected);
|
||||||
throw InitStageException(*me_, "Need 2 children at least.");
|
switch (interfaceFlags()) {
|
||||||
|
|
||||||
InterfaceFlags required = ParallelContainerBasePrivate::requiredInterface();
|
|
||||||
|
|
||||||
// all children need to share a common interface
|
|
||||||
for (const Stage::pointer& stage : children()) {
|
|
||||||
InterfaceFlags current = stage->pimpl()->requiredInterface();
|
|
||||||
if (current != required)
|
|
||||||
throw InitStageException(*stage, "Interface doesn't match the common one.");
|
|
||||||
}
|
|
||||||
|
|
||||||
switch (required) {
|
|
||||||
case PROPAGATE_FORWARDS:
|
case PROPAGATE_FORWARDS:
|
||||||
case PROPAGATE_BACKWARDS:
|
case PROPAGATE_BACKWARDS:
|
||||||
case UNKNOWN:
|
case UNKNOWN:
|
||||||
break; // these are supported
|
break; // these are supported
|
||||||
|
|
||||||
case GENERATE:
|
case GENERATE:
|
||||||
throw InitStageException(*me_, "Generator stages not yet supported.");
|
throw InitStageException(*me_, "Generator stages not yet supported.");
|
||||||
case CONNECT:
|
case CONNECT:
|
||||||
@ -963,7 +776,6 @@ InterfaceFlags MergerPrivate::requiredInterface() const {
|
|||||||
default:
|
default:
|
||||||
throw InitStageException(*me_, "Children's interface not supported.");
|
throw InitStageException(*me_, "Children's interface not supported.");
|
||||||
}
|
}
|
||||||
return required;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {}
|
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {}
|
||||||
|
|||||||
@ -39,15 +39,43 @@
|
|||||||
#include <moveit/task_constructor/container_p.h>
|
#include <moveit/task_constructor/container_p.h>
|
||||||
#include <moveit/task_constructor/introspection.h>
|
#include <moveit/task_constructor/introspection.h>
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
|
||||||
|
#include <ros/console.h>
|
||||||
|
|
||||||
|
#include <boost/format.hpp>
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <ros/console.h>
|
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
|
|
||||||
|
template <>
|
||||||
|
const char* flowSymbol<START_IF_MASK>(InterfaceFlags f) {
|
||||||
|
f = f & START_IF_MASK;
|
||||||
|
if (f == READS_START)
|
||||||
|
return "→";
|
||||||
|
if (f == WRITES_PREV_END)
|
||||||
|
return "←";
|
||||||
|
if (f == UNKNOWN)
|
||||||
|
return "?";
|
||||||
|
return "↔";
|
||||||
|
}
|
||||||
|
|
||||||
|
template <>
|
||||||
|
const char* flowSymbol<END_IF_MASK>(InterfaceFlags f) {
|
||||||
|
f = f & END_IF_MASK;
|
||||||
|
if (f == READS_END)
|
||||||
|
return "←";
|
||||||
|
if (f == WRITES_NEXT_START)
|
||||||
|
return "→";
|
||||||
|
if (f == UNKNOWN)
|
||||||
|
return "?";
|
||||||
|
return "↔";
|
||||||
|
}
|
||||||
|
|
||||||
void InitStageException::push_back(const Stage& stage, const std::string& msg) {
|
void InitStageException::push_back(const Stage& stage, const std::string& msg) {
|
||||||
errors_.emplace_back(std::make_pair(&stage, msg));
|
errors_.emplace_back(std::make_pair(&stage, msg));
|
||||||
}
|
}
|
||||||
@ -57,7 +85,7 @@ void InitStageException::append(InitStageException& other) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
const char* InitStageException::what() const noexcept {
|
const char* InitStageException::what() const noexcept {
|
||||||
static const char* msg = "Error initializing stage(s)";
|
static const char* msg = "Error initializing stage(s). ROS_ERROR_STREAM(e) for details.";
|
||||||
return msg;
|
return msg;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -88,8 +116,12 @@ void StagePrivate::validateConnectivity() const {
|
|||||||
// check that the required interface is provided
|
// check that the required interface is provided
|
||||||
InterfaceFlags required = requiredInterface();
|
InterfaceFlags required = requiredInterface();
|
||||||
InterfaceFlags actual = interfaceFlags();
|
InterfaceFlags actual = interfaceFlags();
|
||||||
if ((required & actual) != required)
|
if ((required & actual) != required) {
|
||||||
throw InitStageException(*me(), "required interface is not satisfied");
|
boost::format desc("actual interface %1% %2% does not match required interface %3% %4%");
|
||||||
|
desc % flowSymbol<START_IF_MASK>(actual) % flowSymbol<END_IF_MASK>(actual);
|
||||||
|
desc % flowSymbol<START_IF_MASK>(required) % flowSymbol<END_IF_MASK>(required);
|
||||||
|
throw InitStageException(*me(), desc.str());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
|
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
|
||||||
@ -332,43 +364,6 @@ void Stage::reportPropertyError(const Property::error& e) {
|
|||||||
throw std::runtime_error(oss.str());
|
throw std::runtime_error(oss.str());
|
||||||
}
|
}
|
||||||
|
|
||||||
template <InterfaceFlag own, InterfaceFlag other>
|
|
||||||
const char* direction(const StagePrivate& stage) {
|
|
||||||
InterfaceFlags f = stage.interfaceFlags();
|
|
||||||
|
|
||||||
bool own_if = f & own;
|
|
||||||
bool other_if = f & other;
|
|
||||||
bool reverse = own & START_IF_MASK;
|
|
||||||
if (own_if && other_if)
|
|
||||||
return "<>";
|
|
||||||
if (!own_if && !other_if)
|
|
||||||
return "--";
|
|
||||||
if (other_if ^ reverse)
|
|
||||||
return "->";
|
|
||||||
return "<-";
|
|
||||||
}
|
|
||||||
|
|
||||||
const char* flowSymbol(InterfaceFlags f) {
|
|
||||||
if (f == UNKNOWN)
|
|
||||||
return "?"; // unknown interface
|
|
||||||
|
|
||||||
// f should have either INPUT or OUTPUT flags set (not both)
|
|
||||||
assert(static_cast<bool>(f & START_IF_MASK) ^ static_cast<bool>(f & END_IF_MASK));
|
|
||||||
|
|
||||||
if (f & START_IF_MASK) {
|
|
||||||
if (f == READS_START)
|
|
||||||
return "↓";
|
|
||||||
if (f == WRITES_PREV_END)
|
|
||||||
return "↑";
|
|
||||||
} else if (f & END_IF_MASK) {
|
|
||||||
if (f == READS_END)
|
|
||||||
return "↑";
|
|
||||||
if (f == WRITES_NEXT_START)
|
|
||||||
return "↓";
|
|
||||||
}
|
|
||||||
return "⇅";
|
|
||||||
}
|
|
||||||
|
|
||||||
std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
|
std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
|
||||||
// starts
|
// starts
|
||||||
for (const InterfaceConstPtr& i : { impl.prevEnds(), impl.starts() }) {
|
for (const InterfaceConstPtr& i : { impl.prevEnds(), impl.starts() }) {
|
||||||
@ -379,8 +374,9 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
|
|||||||
os << "-";
|
os << "-";
|
||||||
}
|
}
|
||||||
// trajectories
|
// trajectories
|
||||||
os << std::setw(5) << direction<READS_START, WRITES_PREV_END>(impl) << std::setw(3) << impl.solutions_.size()
|
os << " " << flowSymbol<START_IF_MASK>(impl.interfaceFlags() | impl.requiredInterface()) << " ";
|
||||||
<< std::setw(5) << direction<READS_END, WRITES_NEXT_START>(impl);
|
os << std::setw(3) << impl.solutions_.size();
|
||||||
|
os << " " << flowSymbol<END_IF_MASK>(impl.interfaceFlags() | impl.requiredInterface()) << " ";
|
||||||
// ends
|
// ends
|
||||||
for (const InterfaceConstPtr& i : { impl.ends(), impl.nextStarts() }) {
|
for (const InterfaceConstPtr& i : { impl.ends(), impl.nextStarts() }) {
|
||||||
os << std::setw(3);
|
os << std::setw(3);
|
||||||
@ -398,67 +394,57 @@ ComputeBase::ComputeBase(ComputeBasePrivate* impl) : Stage(impl) {}
|
|||||||
|
|
||||||
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir,
|
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir,
|
||||||
const std::string& name)
|
const std::string& name)
|
||||||
: ComputeBasePrivate(me, name), required_interface_dirs_(dir) {
|
: ComputeBasePrivate(me, name), configured_dir_(dir) {
|
||||||
initInterface(required_interface_dirs_);
|
initInterface(dir);
|
||||||
}
|
}
|
||||||
|
|
||||||
// initialize pull interfaces to match requested propagation directions
|
// initialize pull interfaces to match requested propagation directions
|
||||||
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) {
|
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) {
|
||||||
if (dir & PropagatingEitherWay::FORWARD) {
|
switch (dir) {
|
||||||
if (!starts_) // keep existing interface if possible
|
case PropagatingEitherWay::FORWARD:
|
||||||
starts_.reset(
|
required_interface_ = PROPAGATE_FORWARDS;
|
||||||
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, std::placeholders::_1)));
|
if (!starts_) // keep existing interface if possible
|
||||||
} else {
|
starts_.reset(
|
||||||
starts_.reset();
|
new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedStarts(it); }));
|
||||||
}
|
ends_.reset();
|
||||||
|
return;
|
||||||
if (dir & PropagatingEitherWay::BACKWARD) {
|
case PropagatingEitherWay::BACKWARD:
|
||||||
if (!ends_) // keep existing interface if possible
|
required_interface_ = PROPAGATE_BACKWARDS;
|
||||||
ends_.reset(
|
starts_.reset();
|
||||||
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, std::placeholders::_1)));
|
if (!ends_) // keep existing interface if possible
|
||||||
} else {
|
ends_.reset(new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedEnds(it); }));
|
||||||
ends_.reset();
|
return;
|
||||||
|
case PropagatingEitherWay::AUTO:
|
||||||
|
required_interface_ = UNKNOWN;
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
|
void PropagatingEitherWayPrivate::resolveInterface(InterfaceFlags expected) {
|
||||||
int dir = 0;
|
if (expected == UNKNOWN)
|
||||||
if ((accepted & PROPAGATE_FORWARDS) == PROPAGATE_FORWARDS)
|
throw InitStageException(*me(), "cannot initialize to unknown interface");
|
||||||
dir |= PropagatingEitherWay::FORWARD;
|
|
||||||
if ((accepted & PROPAGATE_BACKWARDS) == PROPAGATE_BACKWARDS)
|
|
||||||
dir |= PropagatingEitherWay::BACKWARD;
|
|
||||||
initInterface(PropagatingEitherWay::Direction(dir));
|
|
||||||
}
|
|
||||||
|
|
||||||
void PropagatingEitherWayPrivate::validateConnectivity() const {
|
auto dir = PropagatingEitherWay::AUTO;
|
||||||
InterfaceFlags required = requiredInterface();
|
if ((expected & START_IF_MASK) == READS_START || (expected & END_IF_MASK) == WRITES_NEXT_START)
|
||||||
InterfaceFlags actual = interfaceFlags();
|
dir = PropagatingEitherWay::FORWARD;
|
||||||
if (actual == UNKNOWN)
|
else if ((expected & START_IF_MASK) == WRITES_PREV_END || (expected & END_IF_MASK) == READS_END)
|
||||||
throw InitStageException(*me(), "not connected in any direction");
|
dir = PropagatingEitherWay::BACKWARD;
|
||||||
|
else {
|
||||||
InitStageException errors;
|
boost::format desc("propagator cannot satisfy expected interface %1% %2%");
|
||||||
if ((actual & READS_START) && !(actual & WRITES_NEXT_START))
|
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
|
||||||
errors.push_back(*me(), "Cannot push forwards");
|
throw InitStageException(*me(), desc.str());
|
||||||
if ((actual & READS_END) && !(actual & WRITES_PREV_END))
|
}
|
||||||
errors.push_back(*me(), "Cannot push backwards");
|
if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) {
|
||||||
|
boost::format desc("configured interface (%1% %2%) does not match expected one (%3% %4%)");
|
||||||
if (required_interface_dirs_ == PropagatingEitherWay::BOTHWAY && (required & actual) != required)
|
desc % flowSymbol<START_IF_MASK>(required_interface_) % flowSymbol<END_IF_MASK>(required_interface_);
|
||||||
ROS_WARN_STREAM_NAMED("PropagatingEitherWay", "Cannot propagate "
|
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
|
||||||
<< (actual & PROPAGATE_FORWARDS ? "backwards" : "forwards"));
|
throw InitStageException(*me(), desc.str());
|
||||||
|
}
|
||||||
if (errors)
|
initInterface(dir);
|
||||||
throw errors;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
|
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
|
||||||
InterfaceFlags f;
|
return required_interface_;
|
||||||
if (required_interface_dirs_ & PropagatingEitherWay::FORWARD)
|
|
||||||
f |= PROPAGATE_FORWARDS;
|
|
||||||
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
|
|
||||||
f |= PROPAGATE_BACKWARDS;
|
|
||||||
// if required_interface_dirs_ == AUTO, we don't require an interface
|
|
||||||
// but the parent container auto-derives the propagation direction
|
|
||||||
return f;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
|
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
|
||||||
@ -516,26 +502,12 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) :
|
|||||||
|
|
||||||
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) {
|
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) {
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
if (impl->required_interface_dirs_ == dir)
|
if (impl->configured_dir_ == dir)
|
||||||
return;
|
return;
|
||||||
if (impl->prevEnds() || impl->nextStarts())
|
if (impl->configured_dir_ != AUTO)
|
||||||
throw std::runtime_error("Cannot change direction after being connected");
|
throw std::runtime_error("Cannot change direction after being connected");
|
||||||
impl->required_interface_dirs_ = dir;
|
impl->configured_dir_ = dir;
|
||||||
impl->initInterface(impl->required_interface_dirs_);
|
impl->initInterface(dir);
|
||||||
}
|
|
||||||
|
|
||||||
void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
|
||||||
Stage::init(robot_model);
|
|
||||||
auto impl = pimpl();
|
|
||||||
|
|
||||||
// In AUTO-mode, i.e. when auto-detecting direction of propagation from context,
|
|
||||||
// pretend that we offer both interface directions during init().
|
|
||||||
// This is needed due to a chicken-egg-problem: interface auto-detection requires
|
|
||||||
// the context (external pushing interfaces prevEnds, nextStarts) to be set,
|
|
||||||
// while they are ony set if we detected the correct interface...
|
|
||||||
if (impl->required_interface_dirs_ == AUTO)
|
|
||||||
impl->initInterface(BOTHWAY);
|
|
||||||
// otherwise the interface is already fixed and well-defined
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
|
void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
|
||||||
@ -555,7 +527,7 @@ PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward* me, con
|
|||||||
PropagatingForward::PropagatingForward(const std::string& name)
|
PropagatingForward::PropagatingForward(const std::string& name)
|
||||||
: PropagatingEitherWay(new PropagatingForwardPrivate(this, name)) {}
|
: PropagatingEitherWay(new PropagatingForwardPrivate(this, name)) {}
|
||||||
|
|
||||||
void PropagatingForward::computeBackward(const InterfaceState& to) {
|
void PropagatingForward::computeBackward(const InterfaceState& /* to */) {
|
||||||
assert(false); // This should never be called
|
assert(false); // This should never be called
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -568,14 +540,14 @@ PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward* me,
|
|||||||
PropagatingBackward::PropagatingBackward(const std::string& name)
|
PropagatingBackward::PropagatingBackward(const std::string& name)
|
||||||
: PropagatingEitherWay(new PropagatingBackwardPrivate(this, name)) {}
|
: PropagatingEitherWay(new PropagatingBackwardPrivate(this, name)) {}
|
||||||
|
|
||||||
void PropagatingBackward::computeForward(const InterfaceState& from) {
|
void PropagatingBackward::computeForward(const InterfaceState& /* from */) {
|
||||||
assert(false); // This should never be called
|
assert(false); // This should never be called
|
||||||
}
|
}
|
||||||
|
|
||||||
GeneratorPrivate::GeneratorPrivate(Generator* me, const std::string& name) : ComputeBasePrivate(me, name) {}
|
GeneratorPrivate::GeneratorPrivate(Generator* me, const std::string& name) : ComputeBasePrivate(me, name) {}
|
||||||
|
|
||||||
InterfaceFlags GeneratorPrivate::requiredInterface() const {
|
InterfaceFlags GeneratorPrivate::requiredInterface() const {
|
||||||
return InterfaceFlags({ WRITES_NEXT_START, WRITES_PREV_END });
|
return InterfaceFlags(GENERATE);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool GeneratorPrivate::canCompute() const {
|
bool GeneratorPrivate::canCompute() const {
|
||||||
|
|||||||
@ -261,9 +261,7 @@ void Task::init() {
|
|||||||
// and *afterwards* initialize all children recursively
|
// and *afterwards* initialize all children recursively
|
||||||
stages()->init(impl->robot_model_);
|
stages()->init(impl->robot_model_);
|
||||||
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
||||||
stages()->pimpl()->pruneInterface(InterfaceFlags({ GENERATE }));
|
stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE }));
|
||||||
// and *finally* validate connectivity
|
|
||||||
stages()->pimpl()->validateConnectivity();
|
|
||||||
|
|
||||||
// provide introspection instance to all stages
|
// provide introspection instance to all stages
|
||||||
impl->setIntrospection(impl->introspection_.get());
|
impl->setIntrospection(impl->introspection_.get());
|
||||||
|
|||||||
@ -11,12 +11,14 @@
|
|||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
|
static unsigned int mock_id = 0;
|
||||||
|
|
||||||
class GeneratorMockup : public Generator
|
class GeneratorMockup : public Generator
|
||||||
{
|
{
|
||||||
int runs = 0;
|
int runs = 0;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
GeneratorMockup(int runs = 0) : Generator("generator"), runs(runs) {}
|
GeneratorMockup(int runs = 0) : Generator("generator " + std::to_string(++mock_id)), runs(runs) {}
|
||||||
bool canCompute() const override { return runs > 0; }
|
bool canCompute() const override { return runs > 0; }
|
||||||
void compute() override {
|
void compute() override {
|
||||||
if (runs > 0)
|
if (runs > 0)
|
||||||
@ -27,10 +29,11 @@ public:
|
|||||||
class MonitoringGeneratorMockup : public MonitoringGenerator
|
class MonitoringGeneratorMockup : public MonitoringGenerator
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
MonitoringGeneratorMockup(Stage* monitored) : MonitoringGenerator("monitoring generator", monitored) {}
|
MonitoringGeneratorMockup(Stage* monitored)
|
||||||
|
: MonitoringGenerator("monitoring generator " + std::to_string(++mock_id), monitored) {}
|
||||||
bool canCompute() const override { return false; }
|
bool canCompute() const override { return false; }
|
||||||
void compute() override {}
|
void compute() override {}
|
||||||
void onNewSolution(const SolutionBase& s) override {}
|
void onNewSolution(const SolutionBase&) override {}
|
||||||
};
|
};
|
||||||
|
|
||||||
class PropagatorMockup : public PropagatingEitherWay
|
class PropagatorMockup : public PropagatingEitherWay
|
||||||
@ -39,12 +42,13 @@ class PropagatorMockup : public PropagatingEitherWay
|
|||||||
int bw_runs = 0;
|
int bw_runs = 0;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
PropagatorMockup(int fw = 0, int bw = 0) : PropagatingEitherWay("either way"), fw_runs(fw), bw_runs(bw) {}
|
PropagatorMockup(int fw = 0, int bw = 0)
|
||||||
void computeForward(const InterfaceState& from) override {
|
: PropagatingEitherWay("propagate " + std::to_string(++mock_id)), fw_runs(fw), bw_runs(bw) {}
|
||||||
|
void computeForward(const InterfaceState& /* from */) override {
|
||||||
if (fw_runs > 0)
|
if (fw_runs > 0)
|
||||||
--fw_runs;
|
--fw_runs;
|
||||||
}
|
}
|
||||||
void computeBackward(const InterfaceState& from) override {
|
void computeBackward(const InterfaceState& /* from */) override {
|
||||||
if (bw_runs > 0)
|
if (bw_runs > 0)
|
||||||
--bw_runs;
|
--bw_runs;
|
||||||
}
|
}
|
||||||
@ -54,7 +58,7 @@ class ForwardMockup : public PropagatorMockup
|
|||||||
public:
|
public:
|
||||||
ForwardMockup(int runs = 0) : PropagatorMockup(runs, 0) {
|
ForwardMockup(int runs = 0) : PropagatorMockup(runs, 0) {
|
||||||
restrictDirection(FORWARD);
|
restrictDirection(FORWARD);
|
||||||
setName("forward");
|
setName("forward " + std::to_string(mock_id));
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
class BackwardMockup : public PropagatorMockup
|
class BackwardMockup : public PropagatorMockup
|
||||||
@ -62,7 +66,7 @@ class BackwardMockup : public PropagatorMockup
|
|||||||
public:
|
public:
|
||||||
BackwardMockup(int runs = 0) : PropagatorMockup(0, runs) {
|
BackwardMockup(int runs = 0) : PropagatorMockup(0, runs) {
|
||||||
restrictDirection(BACKWARD);
|
restrictDirection(BACKWARD);
|
||||||
setName("backward");
|
setName("backward " + std::to_string(mock_id));
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -71,8 +75,8 @@ class ConnectMockup : public Connecting
|
|||||||
int runs = 0;
|
int runs = 0;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
ConnectMockup(int runs = 0) : Connecting("connect"), runs(runs) {}
|
ConnectMockup(int runs = 0) : Connecting("connect " + std::to_string(++mock_id)), runs(runs) {}
|
||||||
void compute(const InterfaceState& from, const InterfaceState& to) override {
|
void compute(const InterfaceState& /* from */, const InterfaceState& /* to */) override {
|
||||||
if (runs > 0)
|
if (runs > 0)
|
||||||
--runs;
|
--runs;
|
||||||
}
|
}
|
||||||
@ -84,32 +88,25 @@ enum StageType
|
|||||||
FW,
|
FW,
|
||||||
BW,
|
BW,
|
||||||
ANY,
|
ANY,
|
||||||
BOTH,
|
|
||||||
CONN
|
CONN
|
||||||
};
|
};
|
||||||
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
|
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
|
||||||
for (StageType t : types) {
|
for (StageType t : types) {
|
||||||
switch (t) {
|
switch (t) {
|
||||||
case GEN:
|
case GEN:
|
||||||
c.insert(std::make_unique<GeneratorMockup>(runs));
|
c.add(std::make_unique<GeneratorMockup>(runs));
|
||||||
break;
|
break;
|
||||||
case FW:
|
case FW:
|
||||||
c.insert(std::make_unique<ForwardMockup>(runs));
|
c.add(std::make_unique<ForwardMockup>(runs));
|
||||||
break;
|
break;
|
||||||
case BW:
|
case BW:
|
||||||
c.insert(std::make_unique<BackwardMockup>(runs));
|
c.add(std::make_unique<BackwardMockup>(runs));
|
||||||
break;
|
break;
|
||||||
case ANY:
|
case ANY:
|
||||||
c.insert(std::make_unique<PropagatorMockup>(runs, runs));
|
c.add(std::make_unique<PropagatorMockup>(runs, runs));
|
||||||
break;
|
break;
|
||||||
case BOTH: {
|
|
||||||
auto s = std::make_unique<PropagatorMockup>(runs, runs);
|
|
||||||
s->restrictDirection(PropagatingEitherWay::BOTHWAY);
|
|
||||||
c.insert(std::move(s));
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case CONN:
|
case CONN:
|
||||||
c.insert(std::make_unique<ConnectMockup>(runs));
|
c.add(std::make_unique<ConnectMockup>(runs));
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -130,13 +127,13 @@ TEST(ContainerBase, positionForInsert) {
|
|||||||
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
|
||||||
EXPECT_EQ(impl->childByIndex(-2, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(-2, true), impl->children().end());
|
||||||
|
|
||||||
s.insert(std::make_unique<NamedStage>("0"));
|
s.add(std::make_unique<NamedStage>("0"));
|
||||||
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
|
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
|
||||||
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
|
||||||
EXPECT_STREQ((*impl->childByIndex(-2, true))->name().c_str(), "0");
|
EXPECT_STREQ((*impl->childByIndex(-2, true))->name().c_str(), "0");
|
||||||
EXPECT_EQ(impl->childByIndex(-3, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(-3, true), impl->children().end());
|
||||||
|
|
||||||
s.insert(std::make_unique<NamedStage>("1"));
|
s.add(std::make_unique<NamedStage>("1"));
|
||||||
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
|
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
|
||||||
EXPECT_STREQ((*impl->childByIndex(1, true))->name().c_str(), "1");
|
EXPECT_STREQ((*impl->childByIndex(1, true))->name().c_str(), "1");
|
||||||
EXPECT_EQ(impl->childByIndex(2, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(2, true), impl->children().end());
|
||||||
@ -147,15 +144,16 @@ TEST(ContainerBase, positionForInsert) {
|
|||||||
EXPECT_EQ(impl->childByIndex(-4, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(-4, true), impl->children().end());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* TODO: remove interface as it returns raw pointers */
|
||||||
TEST(ContainerBase, findChild) {
|
TEST(ContainerBase, findChild) {
|
||||||
SerialContainer s, *c2;
|
SerialContainer s, *c2;
|
||||||
Stage *a, *b, *c1, *d;
|
Stage *a, *b, *c1, *d;
|
||||||
s.insert(Stage::pointer(a = new NamedStage("a")));
|
s.add(Stage::pointer(a = new NamedStage("a")));
|
||||||
s.insert(Stage::pointer(b = new NamedStage("b")));
|
s.add(Stage::pointer(b = new NamedStage("b")));
|
||||||
s.insert(Stage::pointer(c1 = new NamedStage("c")));
|
s.add(Stage::pointer(c1 = new NamedStage("c")));
|
||||||
auto sub = Stage::pointer(c2 = new SerialContainer("c"));
|
auto sub = ContainerBase::pointer(c2 = new SerialContainer("c"));
|
||||||
c2->insert(Stage::pointer(d = new NamedStage("d")));
|
sub->add(Stage::pointer(d = new NamedStage("d")));
|
||||||
s.insert(std::move(sub));
|
s.add(std::move(sub));
|
||||||
|
|
||||||
EXPECT_EQ(s.findChild("a"), a);
|
EXPECT_EQ(s.findChild("a"), a);
|
||||||
EXPECT_EQ(s.findChild("b"), b);
|
EXPECT_EQ(s.findChild("b"), b);
|
||||||
@ -189,27 +187,42 @@ protected:
|
|||||||
impl->setPrevEnds(InterfacePtr());
|
impl->setPrevEnds(InterfacePtr());
|
||||||
pushInterface(start, end);
|
pushInterface(start, end);
|
||||||
}
|
}
|
||||||
|
void initContainer(const std::initializer_list<StageType>& types = {}) {
|
||||||
|
container.clear();
|
||||||
|
mock_id = 0;
|
||||||
|
append(container, types);
|
||||||
|
}
|
||||||
|
|
||||||
|
void validateInit(bool start, bool end, bool expect_failure) { validateInit(start, end, {}, expect_failure); }
|
||||||
|
|
||||||
void validateInit(bool start, bool end, const std::initializer_list<StageType>& types, bool expect_failure) {
|
void validateInit(bool start, bool end, const std::initializer_list<StageType>& types, bool expect_failure) {
|
||||||
reset(start, end);
|
reset(start, end);
|
||||||
append(container, types);
|
append(container, types);
|
||||||
try {
|
try {
|
||||||
container.init(robot_model);
|
container.init(robot_model);
|
||||||
// prune pull interfaces based on provided external interface (start, end)
|
// resolve interfaces based on provided external interface (start, end)
|
||||||
InterfaceFlags accepted;
|
InterfaceFlags expected;
|
||||||
if (start)
|
if (start)
|
||||||
accepted |= WRITES_PREV_END;
|
expected |= WRITES_PREV_END;
|
||||||
|
else
|
||||||
|
expected |= READS_START;
|
||||||
|
|
||||||
if (end)
|
if (end)
|
||||||
accepted |= WRITES_NEXT_START;
|
expected |= WRITES_NEXT_START;
|
||||||
container.pimpl()->pruneInterface(accepted);
|
else
|
||||||
|
expected |= READS_END;
|
||||||
|
|
||||||
|
container.pimpl()->resolveInterface(expected);
|
||||||
container.pimpl()->validateConnectivity();
|
container.pimpl()->validateConnectivity();
|
||||||
if (!expect_failure)
|
if (!expect_failure)
|
||||||
return; // as expected
|
return; // as expected
|
||||||
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container;
|
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container;
|
||||||
} catch (const InitStageException& e) {
|
} catch (const InitStageException& e) {
|
||||||
if (expect_failure)
|
if (expect_failure) {
|
||||||
|
std::cout << "Expected " << e << "\n" << container;
|
||||||
return; // as expected
|
return; // as expected
|
||||||
ADD_FAILURE() << "unexpected init failure: \n" << e << "\n" << container;
|
}
|
||||||
|
ADD_FAILURE() << "Unexpected " << e << "\n" << container;
|
||||||
} catch (const std::exception& e) {
|
} catch (const std::exception& e) {
|
||||||
ADD_FAILURE() << "unexpected exception thrown:\n" << e.what();
|
ADD_FAILURE() << "unexpected exception thrown:\n" << e.what();
|
||||||
} catch (...) {
|
} catch (...) {
|
||||||
@ -287,14 +300,14 @@ TEST_F(SerialTest, insertion_order) {
|
|||||||
#define EXPECT_INIT_FAILURE(start, end, ...) \
|
#define EXPECT_INIT_FAILURE(start, end, ...) \
|
||||||
{ \
|
{ \
|
||||||
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
|
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
|
||||||
container.clear(); \
|
initContainer(); \
|
||||||
validateInit(start, end, { __VA_ARGS__ }, true); \
|
validateInit(start, end, { __VA_ARGS__ }, true); \
|
||||||
}
|
}
|
||||||
|
|
||||||
#define EXPECT_INIT_SUCCESS(start, end, ...) \
|
#define EXPECT_INIT_SUCCESS(start, end, ...) \
|
||||||
{ \
|
{ \
|
||||||
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
|
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
|
||||||
container.clear(); \
|
initContainer(); \
|
||||||
validateInit(start, end, { __VA_ARGS__ }, false); \
|
validateInit(start, end, { __VA_ARGS__ }, false); \
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -310,8 +323,10 @@ TEST_F(SerialTest, init_connecting) {
|
|||||||
EXPECT_INIT_SUCCESS(false, false, CONN);
|
EXPECT_INIT_SUCCESS(false, false, CONN);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
||||||
|
|
||||||
|
// external interface does not match CONNECT
|
||||||
|
EXPECT_INIT_FAILURE(false, true, CONN);
|
||||||
|
EXPECT_INIT_FAILURE(true, false, CONN);
|
||||||
EXPECT_INIT_FAILURE(true, true, CONN);
|
EXPECT_INIT_FAILURE(true, true, CONN);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({ CONNECT, GENERATE }));
|
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected
|
EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected
|
||||||
}
|
}
|
||||||
@ -320,11 +335,38 @@ TEST_F(SerialTest, init_generator) {
|
|||||||
EXPECT_INIT_SUCCESS(true, true, GEN);
|
EXPECT_INIT_SUCCESS(true, true, GEN);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, GEN); // generator wants to push, but container cannot propagate pushes
|
// external interface does not match GENERATE
|
||||||
|
EXPECT_INIT_FAILURE(false, false, GEN);
|
||||||
|
EXPECT_INIT_FAILURE(false, true, GEN);
|
||||||
|
EXPECT_INIT_FAILURE(true, false, GEN);
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(true, true, GEN, GEN); // two generator stages cannot be connected
|
EXPECT_INIT_FAILURE(true, true, GEN, GEN); // two generator stages cannot be connected
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST_F(SerialTest, keep_configured_propagator_interface) {
|
||||||
|
// configuring an ANY propagator should work
|
||||||
|
EXPECT_INIT_SUCCESS(false, true, ANY);
|
||||||
|
EXPECT_EQ(container.pimpl()->children().front()->pimpl()->requiredInterface(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
|
|
||||||
|
// multiple times
|
||||||
|
validateInit(false, true, false);
|
||||||
|
EXPECT_EQ(container.pimpl()->children().front()->pimpl()->requiredInterface(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
|
// and with another interface direction as well
|
||||||
|
validateInit(true, false, false);
|
||||||
|
EXPECT_EQ(container.pimpl()->children().front()->pimpl()->requiredInterface(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
|
||||||
|
// configuring a FORWARD propagator should work in forward direction
|
||||||
|
EXPECT_INIT_SUCCESS(false, true, FW);
|
||||||
|
// but fail when initialized in backward direction
|
||||||
|
validateInit(true, false, true);
|
||||||
|
EXPECT_EQ(container.pimpl()->children().front()->pimpl()->requiredInterface(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
|
|
||||||
|
// same with BACKWARD propagator
|
||||||
|
EXPECT_INIT_SUCCESS(true, false, BW);
|
||||||
|
validateInit(false, true, true);
|
||||||
|
EXPECT_EQ(container.pimpl()->children().front()->pimpl()->requiredInterface(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
}
|
||||||
|
|
||||||
TEST_F(SerialTest, init_forward) {
|
TEST_F(SerialTest, init_forward) {
|
||||||
// container interface doesn't match children
|
// container interface doesn't match children
|
||||||
EXPECT_INIT_FAILURE(false, false, FW);
|
EXPECT_INIT_FAILURE(false, false, FW);
|
||||||
@ -359,12 +401,6 @@ TEST_F(SerialTest, init_forward) {
|
|||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, false, BW, ANY);
|
EXPECT_INIT_SUCCESS(true, false, BW, ANY);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
|
||||||
// BOTH requires propagation in both directions, while FW/BW can only match one dir
|
|
||||||
EXPECT_INIT_FAILURE(true, true, BOTH, FW);
|
|
||||||
EXPECT_INIT_FAILURE(true, true, FW, BOTH);
|
|
||||||
EXPECT_INIT_FAILURE(true, true, BOTH, BW);
|
|
||||||
EXPECT_INIT_FAILURE(true, true, BW, BOTH);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SerialTest, init_backward) {
|
TEST_F(SerialTest, init_backward) {
|
||||||
@ -385,6 +421,18 @@ TEST_F(SerialTest, init_backward) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SerialTest, interface_detection) {
|
TEST_F(SerialTest, interface_detection) {
|
||||||
|
// ANY resolves in either direction
|
||||||
|
EXPECT_INIT_SUCCESS(false, true, ANY);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
|
EXPECT_INIT_SUCCESS(true, false, ANY);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
|
||||||
|
EXPECT_INIT_SUCCESS(true, true, GEN, ANY);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||||
|
|
||||||
|
EXPECT_INIT_SUCCESS(true, true, ANY, GEN);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||||
|
|
||||||
// derive propagation direction from inner generator
|
// derive propagation direction from inner generator
|
||||||
EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> ->
|
EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> ->
|
||||||
auto it = container.pimpl()->children().begin();
|
auto it = container.pimpl()->children().begin();
|
||||||
@ -393,12 +441,6 @@ TEST_F(SerialTest, interface_detection) {
|
|||||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, GEN, ANY);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, ANY, GEN);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
|
||||||
|
|
||||||
// derive propagation direction from inner connector
|
// derive propagation direction from inner connector
|
||||||
EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <-
|
EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <-
|
||||||
it = container.pimpl()->children().begin();
|
it = container.pimpl()->children().begin();
|
||||||
@ -414,9 +456,9 @@ TEST_F(SerialTest, interface_detection) {
|
|||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
||||||
|
|
||||||
// derive propagation direction from outer interface
|
// derive propagation direction from outer interface
|
||||||
EXPECT_INIT_SUCCESS(false, true, ANY); // should be pruned to FW
|
EXPECT_INIT_SUCCESS(false, true, ANY); // should be resolved to FW
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
EXPECT_INIT_SUCCESS(true, false, ANY); // should be pruned to BW
|
EXPECT_INIT_SUCCESS(true, false, ANY); // should be resolved to BW
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> ->
|
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> ->
|
||||||
@ -431,109 +473,74 @@ TEST_F(SerialTest, interface_detection) {
|
|||||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <>
|
EXPECT_INIT_FAILURE(true, true, ANY, ANY); // no generator
|
||||||
it = container.pimpl()->children().begin();
|
|
||||||
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- --
|
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // no connect
|
||||||
it = container.pimpl()->children().begin();
|
}
|
||||||
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), UNKNOWN);
|
|
||||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), UNKNOWN);
|
template <typename T>
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), UNKNOWN);
|
Stage::pointer make(const std::string& name, const std::initializer_list<StageType>& children, int runs = 0) {
|
||||||
|
auto container = new T(name);
|
||||||
|
append(*container, children);
|
||||||
|
return Stage::pointer(container);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SerialTest, nested_interface_detection) {
|
TEST_F(SerialTest, nested_interface_detection) {
|
||||||
SerialContainer* inner;
|
|
||||||
// direction imposed from outer generator
|
// direction imposed from outer generator
|
||||||
container.clear();
|
initContainer({ GEN, ANY });
|
||||||
inner = new SerialContainer("inner serial");
|
container.add(make<SerialContainer>("inner serial", { ANY, ANY }));
|
||||||
append(*inner, { ANY, ANY });
|
append(container, { ANY });
|
||||||
append(container, { GEN, ANY, ANY });
|
|
||||||
container.insert(Stage::pointer(inner), -2);
|
|
||||||
{
|
{
|
||||||
SCOPED_TRACE("GEN - ANY - inner - ANY");
|
SCOPED_TRACE("GEN - ANY - Serial( ANY - ANY ) - ANY");
|
||||||
validateInit(true, true, {}, false);
|
validateInit(true, true, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
// direction imposed from inner generator
|
// direction imposed from inner generator
|
||||||
container.clear();
|
initContainer({ ANY });
|
||||||
inner = new SerialContainer("inner serial");
|
container.add(make<SerialContainer>("inner serial", { ANY, GEN, ANY }));
|
||||||
append(*inner, { ANY, GEN, ANY });
|
append(container, { ANY });
|
||||||
append(container, { ANY, ANY });
|
|
||||||
container.insert(Stage::pointer(inner), -2);
|
|
||||||
{
|
{
|
||||||
SCOPED_TRACE("ANY - inner - ANY");
|
SCOPED_TRACE("ANY - Serial( ANY - GEN - ANY ) - ANY");
|
||||||
validateInit(true, true, {}, false);
|
validateInit(true, true, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
container.clear();
|
initContainer();
|
||||||
inner = new SerialContainer("inner serial");
|
container.add(make<SerialContainer>("inner serial", { GEN, ANY }));
|
||||||
append(*inner, { GEN, ANY });
|
append(container, { ANY });
|
||||||
container.insert(Stage::pointer(inner));
|
|
||||||
{
|
{
|
||||||
SCOPED_TRACE("inner - ANY");
|
SCOPED_TRACE("Serial( GEN - ANY ) - ANY");
|
||||||
validateInit(true, true, { ANY }, false);
|
validateInit(true, true, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
// outer and inner generators conflict with each other
|
// outer and inner generators conflict with each other
|
||||||
container.clear();
|
initContainer({ GEN, ANY });
|
||||||
inner = new SerialContainer("inner serial");
|
container.add(make<SerialContainer>("inner serial", { ANY, GEN, ANY }));
|
||||||
append(*inner, { ANY, GEN, ANY });
|
append(container, { ANY });
|
||||||
append(container, { GEN, ANY, ANY });
|
|
||||||
container.insert(Stage::pointer(inner), -2);
|
|
||||||
{
|
{
|
||||||
SCOPED_TRACE("GEN - ANY - inner - ANY");
|
SCOPED_TRACE("GEN - ANY - Serial( ANY - GEN - ANY ) - ANY");
|
||||||
validateInit(true, true, {}, true);
|
validateInit(true, true, true); // expected to fail
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(SerialTest, nested_parallel) {
|
||||||
|
initContainer({ GEN });
|
||||||
|
container.add(make<Alternatives>("inner alternatives", { ANY, ANY }));
|
||||||
|
{
|
||||||
|
SCOPED_TRACE("GEN - Alternatives( ANY - ANY )");
|
||||||
|
validateInit(true, true, false);
|
||||||
|
}
|
||||||
|
|
||||||
|
initContainer({ GEN });
|
||||||
|
container.add(make<Fallbacks>("inner alternatives", { ANY, FW }));
|
||||||
|
{
|
||||||
|
SCOPED_TRACE("GEN - Fallback( ANY - FW )");
|
||||||
|
validateInit(true, true, false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
class ParallelTest : public InitTest<Alternatives>
|
class ParallelTest : public InitTest<Alternatives>
|
||||||
{};
|
{};
|
||||||
|
|
||||||
TEST_F(ParallelTest, init_propagating) {
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, BW, FW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, BOTH, BOTH);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, BW, BOTH);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, FW, BOTH);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, FW, BOTH, FW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, FW, BW, FW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, BW, FW, BW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(false, true, FW, FW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, false, BW, BW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS);
|
|
||||||
|
|
||||||
// external interface doesn't match derived
|
|
||||||
EXPECT_INIT_FAILURE(false, false, BOTH);
|
|
||||||
EXPECT_INIT_FAILURE(false, true, BOTH);
|
|
||||||
EXPECT_INIT_FAILURE(true, false, BOTH);
|
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, BW, BW);
|
|
||||||
EXPECT_INIT_FAILURE(false, true, BW, BW);
|
|
||||||
EXPECT_INIT_FAILURE(true, true, BW, BW);
|
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, FW, FW);
|
|
||||||
EXPECT_INIT_FAILURE(true, false, FW, FW);
|
|
||||||
EXPECT_INIT_FAILURE(true, true, FW, FW);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_F(ParallelTest, init_matching) {
|
TEST_F(ParallelTest, init_matching) {
|
||||||
EXPECT_INIT_SUCCESS(true, true, GEN, GEN);
|
EXPECT_INIT_SUCCESS(true, true, GEN, GEN);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), GENERATE);
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), GENERATE);
|
||||||
@ -562,6 +569,11 @@ TEST_F(ParallelTest, init_mismatching) {
|
|||||||
EXPECT_INIT_FAILURE(false, true, FW, CONN);
|
EXPECT_INIT_FAILURE(false, true, FW, CONN);
|
||||||
EXPECT_INIT_FAILURE(true, true, FW, CONN);
|
EXPECT_INIT_FAILURE(true, true, FW, CONN);
|
||||||
|
|
||||||
|
EXPECT_INIT_FAILURE(false, false, ANY, CONN);
|
||||||
|
EXPECT_INIT_FAILURE(true, false, ANY, CONN);
|
||||||
|
EXPECT_INIT_FAILURE(false, true, ANY, CONN);
|
||||||
|
EXPECT_INIT_FAILURE(true, true, ANY, CONN);
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, BW, GEN);
|
EXPECT_INIT_FAILURE(false, false, BW, GEN);
|
||||||
EXPECT_INIT_FAILURE(true, false, BW, GEN);
|
EXPECT_INIT_FAILURE(true, false, BW, GEN);
|
||||||
EXPECT_INIT_FAILURE(false, true, BW, GEN);
|
EXPECT_INIT_FAILURE(false, true, BW, GEN);
|
||||||
@ -572,20 +584,62 @@ TEST_F(ParallelTest, init_mismatching) {
|
|||||||
EXPECT_INIT_FAILURE(false, true, FW, GEN);
|
EXPECT_INIT_FAILURE(false, true, FW, GEN);
|
||||||
EXPECT_INIT_FAILURE(true, true, FW, GEN);
|
EXPECT_INIT_FAILURE(true, true, FW, GEN);
|
||||||
|
|
||||||
|
EXPECT_INIT_FAILURE(false, false, ANY, GEN);
|
||||||
|
EXPECT_INIT_FAILURE(true, false, ANY, GEN);
|
||||||
|
EXPECT_INIT_FAILURE(false, true, ANY, GEN);
|
||||||
|
EXPECT_INIT_FAILURE(true, true, ANY, GEN);
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, CONN, GEN);
|
EXPECT_INIT_FAILURE(false, false, CONN, GEN);
|
||||||
EXPECT_INIT_FAILURE(true, false, CONN, GEN);
|
EXPECT_INIT_FAILURE(true, false, CONN, GEN);
|
||||||
EXPECT_INIT_FAILURE(false, true, CONN, GEN);
|
EXPECT_INIT_FAILURE(false, true, CONN, GEN);
|
||||||
EXPECT_INIT_FAILURE(true, true, CONN, GEN);
|
EXPECT_INIT_FAILURE(true, true, CONN, GEN);
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, BOTH, CONN);
|
EXPECT_INIT_FAILURE(false, false, BW, FW);
|
||||||
EXPECT_INIT_FAILURE(true, false, BOTH, CONN);
|
EXPECT_INIT_FAILURE(true, false, BW, FW);
|
||||||
EXPECT_INIT_FAILURE(false, true, BOTH, CONN);
|
EXPECT_INIT_FAILURE(false, true, BW, FW);
|
||||||
EXPECT_INIT_FAILURE(true, true, BOTH, CONN);
|
EXPECT_INIT_FAILURE(true, true, BW, FW);
|
||||||
|
}
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, BOTH, GEN);
|
TEST_F(ParallelTest, init_propagating) {
|
||||||
EXPECT_INIT_FAILURE(true, false, BOTH, GEN);
|
EXPECT_INIT_SUCCESS(false, true, FW, FW);
|
||||||
EXPECT_INIT_FAILURE(false, true, BOTH, GEN);
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
|
||||||
EXPECT_INIT_FAILURE(true, true, BOTH, GEN);
|
EXPECT_INIT_FAILURE(false, false, FW, FW);
|
||||||
|
EXPECT_INIT_FAILURE(true, false, FW, FW);
|
||||||
|
EXPECT_INIT_FAILURE(true, true, FW, FW);
|
||||||
|
|
||||||
|
EXPECT_INIT_SUCCESS(true, false, BW, BW);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS);
|
||||||
|
EXPECT_INIT_FAILURE(false, false, BW, BW);
|
||||||
|
EXPECT_INIT_FAILURE(false, true, BW, BW);
|
||||||
|
EXPECT_INIT_FAILURE(true, true, BW, BW);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(ParallelTest, init_any) {
|
||||||
|
EXPECT_INIT_FAILURE(true, true, ANY, ANY); // no generator
|
||||||
|
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // no connector
|
||||||
|
|
||||||
|
// infer container (and children) direction from outside
|
||||||
|
EXPECT_INIT_SUCCESS(false, true, ANY, ANY);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
|
for (const auto& child : container.pimpl()->children())
|
||||||
|
EXPECT_EQ(child->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)) << child->name();
|
||||||
|
|
||||||
|
EXPECT_INIT_SUCCESS(true, false, ANY, ANY);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
for (const auto& child : container.pimpl()->children())
|
||||||
|
EXPECT_EQ(child->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)) << child->name();
|
||||||
|
|
||||||
|
EXPECT_INIT_SUCCESS(true, false, BW, ANY); // infer ANY as BACKWARDS
|
||||||
|
for (const auto& child : container.pimpl()->children())
|
||||||
|
EXPECT_EQ(child->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)) << child->name();
|
||||||
|
|
||||||
|
EXPECT_INIT_SUCCESS(false, true, ANY, FW); // infer ANY as FORWARDS
|
||||||
|
for (const auto& child : container.pimpl()->children())
|
||||||
|
EXPECT_EQ(child->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)) << child->name();
|
||||||
|
|
||||||
|
EXPECT_INIT_SUCCESS(false, true, FW, ANY, FW); // infer ANY as FORWARDS
|
||||||
|
for (const auto& child : container.pimpl()->children())
|
||||||
|
EXPECT_EQ(child->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)) << child->name();
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(Task, move) {
|
TEST(Task, move) {
|
||||||
|
|||||||
46
demo/scripts/grasp.py
Normal file
46
demo/scripts/grasp.py
Normal file
@ -0,0 +1,46 @@
|
|||||||
|
#! /usr/bin/env python
|
||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
|
||||||
|
from std_msgs.msg import Header
|
||||||
|
from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3, PoseStamped, Pose, Point, Quaternion
|
||||||
|
from moveit.task_constructor import core, stages
|
||||||
|
import moveit_commander
|
||||||
|
import rospy
|
||||||
|
import numpy
|
||||||
|
|
||||||
|
from moveit.python_tools import roscpp_init
|
||||||
|
roscpp_init("mtc_tutorial")
|
||||||
|
rospy.init_node('mtc_tutorial_py', anonymous=False)
|
||||||
|
|
||||||
|
def create_object():
|
||||||
|
scene = moveit_commander.PlanningSceneInterface(synchronous=True)
|
||||||
|
scene.remove_world_object();
|
||||||
|
pose = PoseStamped()
|
||||||
|
pose.header.frame_id = "world"
|
||||||
|
pose.pose.position.x = 0.5
|
||||||
|
pose.pose.position.y = numpy.random.uniform(-0.1, 0.1)
|
||||||
|
pose.pose.position.z = 0.16
|
||||||
|
pose.pose.orientation.w = 1.0
|
||||||
|
scene.add_box('object', pose, size=(0.05, 0.05, 0.2))
|
||||||
|
|
||||||
|
group = "panda_arm"
|
||||||
|
eef_frame = "panda_link8"
|
||||||
|
|
||||||
|
# Cartesian interpolation planner
|
||||||
|
cartesian = core.CartesianPath()
|
||||||
|
|
||||||
|
task = core.Task()
|
||||||
|
|
||||||
|
# start from current robot state
|
||||||
|
task.add(stages.CurrentState("current state"))
|
||||||
|
|
||||||
|
move = stages.MoveTo("to object", cartesian)
|
||||||
|
move.group = group
|
||||||
|
header = Header(frame_id = "object")
|
||||||
|
move.setGoal(PoseStamped(header=header, pose=Pose(position=Point(0,0,0.18),orientation=Quaternion(0.92388, -0.38268, 0, 0))))
|
||||||
|
task.add(move)
|
||||||
|
|
||||||
|
create_object()
|
||||||
|
if task.plan():
|
||||||
|
task.publish(task.solutions[0])
|
||||||
|
rospy.spin()
|
||||||
65
demo/scripts/pick.py
Normal file
65
demo/scripts/pick.py
Normal file
@ -0,0 +1,65 @@
|
|||||||
|
#! /usr/bin/env python
|
||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
|
||||||
|
from std_msgs.msg import Header
|
||||||
|
from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3, PoseStamped, Pose, Point, Quaternion
|
||||||
|
from moveit.task_constructor import core, stages
|
||||||
|
import moveit_commander
|
||||||
|
import rospy
|
||||||
|
import numpy
|
||||||
|
|
||||||
|
from moveit.python_tools import roscpp_init
|
||||||
|
roscpp_init("mtc_tutorial")
|
||||||
|
rospy.init_node('mtc_tutorial_py', anonymous=False)
|
||||||
|
|
||||||
|
group = 'panda_arm'
|
||||||
|
eef = 'panda_hand'
|
||||||
|
eef_frame = "panda_link8"
|
||||||
|
|
||||||
|
sampling_planner = core.JointInterpolationPlanner()
|
||||||
|
cartesian_planner = core.CartesianPath()
|
||||||
|
|
||||||
|
task = core.Task()
|
||||||
|
|
||||||
|
task.properties.update({'group': group, 'eef': eef, 'hand': eef, 'hand_grasping_frame': eef, 'ik_frame': eef_frame})
|
||||||
|
|
||||||
|
currstate = stages.CurrentState('current state')
|
||||||
|
#task.add(currstate) # Adding it to the task results in error for argument types in setMonitoredStage in GenerateGraspPose
|
||||||
|
|
||||||
|
open_hand = stages.MoveTo("open hand", sampling_planner)
|
||||||
|
open_hand.group = eef
|
||||||
|
open_hand.setGoal('open')
|
||||||
|
task.add(open_hand)
|
||||||
|
|
||||||
|
connect = stages.Connect('move to pick', [(group, sampling_planner)])
|
||||||
|
connect.timeout = 5
|
||||||
|
connect.properties.configureInitFrom(core.PARENT)
|
||||||
|
task.add(connect)
|
||||||
|
|
||||||
|
grasp = core.SerialContainer('pick object')
|
||||||
|
task.properties.exposeTo(grasp.properties, ['eef', 'hand', 'group', 'ik_frame'])
|
||||||
|
grasp.properties.configureInitFrom(core.PARENT, ['eef', 'hand', 'group', 'ik_frame'])
|
||||||
|
|
||||||
|
approach_object = stages.MoveRelative("approach_object", cartesian_planner)
|
||||||
|
approach_object.properties.update({'marker_ns': 'approach_object', 'link': eef_frame})
|
||||||
|
approach_object.properties.configureInitFrom(core.PARENT, ['group'])
|
||||||
|
approach_object.min_distance = 0.01
|
||||||
|
approach_object.max_distance = 0.1
|
||||||
|
print(approach_object.properties.__getitem__('group')) # Why is this None? how to get properties from within SerialContainer?
|
||||||
|
approach_object.setDirection(TwistStamped(header=Header(frame_id = eef_frame), twist=Twist(linear=Vector3(0,0,0.1))))
|
||||||
|
grasp.insert(approach_object)
|
||||||
|
|
||||||
|
generatepose = stages.GenerateGraspPose('generate grasp pose')
|
||||||
|
generatepose.properties.configureInitFrom(core.PARENT)
|
||||||
|
generatepose.properties.update({'marker_ns': 'grasp_pose'})
|
||||||
|
generatepose.pregrasp = 'open'
|
||||||
|
generatepose.object = 'base'
|
||||||
|
generatepose.angle_delta = numpy.pi/12
|
||||||
|
generatepose.setMonitoredStage(currstate)
|
||||||
|
|
||||||
|
# To be continued
|
||||||
|
task.add(grasp)
|
||||||
|
|
||||||
|
if task.plan():
|
||||||
|
task.publish(task.solutions[0])
|
||||||
|
rospy.spin()
|
||||||
53
notes.org
Normal file
53
notes.org
Normal file
@ -0,0 +1,53 @@
|
|||||||
|
* scheduling
|
||||||
|
There are 2 levels of scheduling:
|
||||||
|
- choosing which state or state pair should be scheduled for execution
|
||||||
|
- choosing which stage should be scheduled for execution
|
||||||
|
|
||||||
|
Main goal is to find small-cost solutions fast.
|
||||||
|
To this end, we need to find a solution at all, i.e. connect start to end.
|
||||||
|
Second, we prefer small-cost solutions.
|
||||||
|
|
||||||
|
First sort by length of trajectory, second by sum of trajectory costs.
|
||||||
|
|
||||||
|
** approach
|
||||||
|
- scheduling priority for stage (default / assigned by user)
|
||||||
|
- fallback: sort stages by priority
|
||||||
|
- evaluate success rate
|
||||||
|
- expected costs of solutions
|
||||||
|
- expected computing time
|
||||||
|
- Container::schedule() -> return ordered list of stages to execute
|
||||||
|
|
||||||
|
* containers / interfaces
|
||||||
|
Basic stages have unique solutions connecting start-end.
|
||||||
|
Containers allow for several solutions connecting start-end:
|
||||||
|
- serial: several pathes might exist
|
||||||
|
- parallel: inherent
|
||||||
|
|
||||||
|
* notation
|
||||||
|
push vs pull connections
|
||||||
|
|
||||||
|
* TODO
|
||||||
|
** modify-ps
|
||||||
|
- std::function API
|
||||||
|
- replace templates with sfniae
|
||||||
|
|
||||||
|
** eef collision
|
||||||
|
- requires modification in MoveIt
|
||||||
|
- disable warnings vs. only consider relevant links (separate function?)
|
||||||
|
|
||||||
|
** MoveTo / MoveRelative
|
||||||
|
- interface: pass goal constraints
|
||||||
|
- fill goal constraints during setup?
|
||||||
|
requires transforms to be specified relative to current robot pose
|
||||||
|
|
||||||
|
** properties
|
||||||
|
- fix plan_pick_pa10: forward grasp property
|
||||||
|
- global type registry for serialization/deserialization functions
|
||||||
|
- use bits as enums to allow for configureInitFrom(PARENT | INTERFACE)
|
||||||
|
** incremental GraspPoseGenerator
|
||||||
|
in pending_ list, store the current angular value
|
||||||
|
** TaskModels
|
||||||
|
- move scene_, display_context_ to BaseTaskModel
|
||||||
|
- use registered property creators for RemoteTaskModel too
|
||||||
|
- deserialize property
|
||||||
|
- call creator function
|
||||||
28
pick+place.org
Normal file
28
pick+place.org
Normal file
@ -0,0 +1,28 @@
|
|||||||
|
* capability params
|
||||||
|
- overall planning timeout
|
||||||
|
- max number of solutions
|
||||||
|
|
||||||
|
** Pick
|
||||||
|
- possible_grasps -> fixed list
|
||||||
|
- instantiate GraspProvider+Task on each call (eventually cache)
|
||||||
|
- object
|
||||||
|
- key-value parameter map (passed on to Action Server)
|
||||||
|
|
||||||
|
* GraspProvider params
|
||||||
|
- eef (hand_group, arm_group, parent link)
|
||||||
|
- hand_group: default from arm_group
|
||||||
|
- arm_group: default from hand_group
|
||||||
|
- grasp frame (PoseStamped rel. zu Link fest zu Tip-Link im Arm)
|
||||||
|
- key-value parameter map (passed on to Action Server)
|
||||||
|
- grasp config
|
||||||
|
- action server name (with default)
|
||||||
|
|
||||||
|
* PlaceProvider params
|
||||||
|
- PlaceLocation[]
|
||||||
|
- surface_name: sample PlaceLocations, disable collision with table during IK
|
||||||
|
|
||||||
|
* GraspMsg
|
||||||
|
- grasp frame?
|
||||||
|
- no-collision links (during closing)
|
||||||
|
- no-collision
|
||||||
|
|
||||||
16
test.py
Normal file
16
test.py
Normal file
@ -0,0 +1,16 @@
|
|||||||
|
import moveit.task_constructor as mtc
|
||||||
|
|
||||||
|
o=mtc.MyObject(42)
|
||||||
|
a=o
|
||||||
|
#print o
|
||||||
|
#print a
|
||||||
|
mtc.access(o)
|
||||||
|
mtc.access(o)
|
||||||
|
mtc.consume(o)
|
||||||
|
mtc.access(o)
|
||||||
|
mtc.access(o)
|
||||||
|
#print o
|
||||||
|
#print a
|
||||||
|
del o
|
||||||
|
del a
|
||||||
|
print "done"
|
||||||
@ -124,7 +124,6 @@ QVariant BaseTaskModel::flowIcon(moveit::task_constructor::InterfaceFlags f) {
|
|||||||
static const QIcon CONNECT_ICON = icons::CONNECT.icon();
|
static const QIcon CONNECT_ICON = icons::CONNECT.icon();
|
||||||
static const QIcon FORWARD_ICON = icons::FORWARD.icon();
|
static const QIcon FORWARD_ICON = icons::FORWARD.icon();
|
||||||
static const QIcon BACKWARD_ICON = icons::BACKWARD.icon();
|
static const QIcon BACKWARD_ICON = icons::BACKWARD.icon();
|
||||||
static const QIcon BOTHWAY_ICON = icons::BOTHWAY.icon();
|
|
||||||
static const QIcon GENERATE_ICON = icons::GENERATE.icon();
|
static const QIcon GENERATE_ICON = icons::GENERATE.icon();
|
||||||
|
|
||||||
if (f == InterfaceFlags(CONNECT))
|
if (f == InterfaceFlags(CONNECT))
|
||||||
@ -133,8 +132,6 @@ QVariant BaseTaskModel::flowIcon(moveit::task_constructor::InterfaceFlags f) {
|
|||||||
return FORWARD_ICON;
|
return FORWARD_ICON;
|
||||||
if (f == InterfaceFlags(PROPAGATE_BACKWARDS))
|
if (f == InterfaceFlags(PROPAGATE_BACKWARDS))
|
||||||
return BACKWARD_ICON;
|
return BACKWARD_ICON;
|
||||||
if (f == PROPAGATE_BOTHWAYS)
|
|
||||||
return BOTHWAY_ICON;
|
|
||||||
if (f == InterfaceFlags(GENERATE))
|
if (f == InterfaceFlags(GENERATE))
|
||||||
return GENERATE_ICON;
|
return GENERATE_ICON;
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user