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)
|
||||
SerialContainer(const std::string& name = "serial container");
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
|
||||
bool canCompute() const override;
|
||||
void compute() override;
|
||||
|
||||
@ -130,8 +128,6 @@ public:
|
||||
PRIVATE_CLASS(ParallelContainerBase)
|
||||
ParallelContainerBase(const std::string& name = "parallel container");
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
|
||||
protected:
|
||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||
|
||||
|
||||
@ -105,6 +105,10 @@ public:
|
||||
|
||||
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
|
||||
bool canCompute() const override;
|
||||
void compute() override;
|
||||
@ -115,21 +119,17 @@ public:
|
||||
protected:
|
||||
ContainerBasePrivate(ContainerBase* me, const std::string& name);
|
||||
|
||||
// Set child's push interfaces: allow pushing if child requires it or
|
||||
// if the interface is unknown: in this case greedily assume a push interface.
|
||||
// If, during pruneInterface() later, we notice that it's not needed, prune there.
|
||||
inline void setChildsPushBackwardInterface(Stage& child) {
|
||||
InterfaceFlags required = child.pimpl()->requiredInterface();
|
||||
bool allowed = (required & WRITES_PREV_END) || (required == UNKNOWN);
|
||||
child.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr());
|
||||
// Set child's push interfaces: allow pushing if child requires it.
|
||||
inline void setChildsPushBackwardInterface(StagePrivate* child) {
|
||||
InterfaceFlags required = child->requiredInterface();
|
||||
bool allowed = (required & WRITES_PREV_END);
|
||||
child->setPrevEnds(allowed ? pending_backward_ : InterfacePtr());
|
||||
}
|
||||
inline void setChildsPushForwardInterface(Stage& child) {
|
||||
InterfaceFlags required = child.pimpl()->requiredInterface();
|
||||
bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN);
|
||||
child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
||||
inline void setChildsPushForwardInterface(StagePrivate* child) {
|
||||
InterfaceFlags required = child->requiredInterface();
|
||||
bool allowed = (required & WRITES_NEXT_START);
|
||||
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
|
||||
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
|
||||
@ -139,6 +139,9 @@ protected:
|
||||
auto& internalToExternalMap() { return internal_to_external_; }
|
||||
const auto& internalToExternalMap() const { return internal_to_external_; }
|
||||
|
||||
// set in resolveInterface()
|
||||
InterfaceFlags required_interface_;
|
||||
|
||||
private:
|
||||
container_type children_;
|
||||
|
||||
@ -166,28 +169,20 @@ class SerialContainerPrivate : public ContainerBasePrivate
|
||||
public:
|
||||
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
|
||||
void pruneInterface(InterfaceFlags accepted) override;
|
||||
void resolveInterface(InterfaceFlags expected) override;
|
||||
// validate connectivity of chain
|
||||
void validateConnectivity() const override;
|
||||
|
||||
private:
|
||||
// connect cur stage to its predecessor and successor
|
||||
bool connect(container_type::const_iterator cur);
|
||||
void reset();
|
||||
|
||||
// called by init() to prune interfaces for children in range [first, last)
|
||||
void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end);
|
||||
// prune interface for children in range [first, last) to given direction
|
||||
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);
|
||||
protected:
|
||||
// connect two neighbors
|
||||
void connect(StagePrivate& stage1, StagePrivate& stage2);
|
||||
|
||||
private:
|
||||
InterfaceFlags required_interface_;
|
||||
// validate that child's interface matches mine (considering start or end only as determined by mask)
|
||||
template <unsigned int mask>
|
||||
void validateInterface(const StagePrivate& child, InterfaceFlags required) const;
|
||||
};
|
||||
PIMPL_FUNCTIONS(SerialContainer)
|
||||
|
||||
@ -219,14 +214,14 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
|
||||
public:
|
||||
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
|
||||
void pruneInterface(InterfaceFlags accepted) override;
|
||||
void resolveInterface(InterfaceFlags expected) override;
|
||||
|
||||
void validateConnectivity() const override;
|
||||
|
||||
protected:
|
||||
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
|
||||
|
||||
private:
|
||||
/// callback for new externally received states
|
||||
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
|
||||
@ -256,7 +251,7 @@ public:
|
||||
typedef std::function<void(SubTrajectory&&)> Spawner;
|
||||
MergerPrivate(Merger* me, const std::string& name);
|
||||
|
||||
InterfaceFlags requiredInterface() const override;
|
||||
void resolveInterface(InterfaceFlags expected) override;
|
||||
|
||||
void onNewPropagateSolution(const SolutionBase& s);
|
||||
void onNewGeneratorSolution(const SolutionBase& s);
|
||||
|
||||
@ -83,13 +83,30 @@ enum InterfaceFlag
|
||||
PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END,
|
||||
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
|
||||
};
|
||||
|
||||
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
|
||||
constexpr InterfaceFlags UNKNOWN;
|
||||
constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
|
||||
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(Stage)
|
||||
@ -253,12 +270,9 @@ public:
|
||||
AUTO = 0x00, // auto-derive direction from context
|
||||
FORWARD = 0x01, // propagate forward only
|
||||
BACKWARD = 0x02, // propagate backward only
|
||||
BOTHWAY = FORWARD | BACKWARD, // propagate both ways
|
||||
};
|
||||
void restrictDirection(Direction dir);
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
|
||||
virtual void computeForward(const InterfaceState& from) = 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())
|
||||
InterfaceFlags interfaceFlags() const;
|
||||
|
||||
/** Interface required by this stage
|
||||
*
|
||||
* If the interface is unknown (because it is auto-detected from context), return 0 */
|
||||
/// Retrieve interface required by this stage, UNKNOWN if auto-detected from context
|
||||
virtual InterfaceFlags requiredInterface() const = 0;
|
||||
|
||||
/** Prune interface to comply with the given propagation direction
|
||||
*
|
||||
* PropagatingEitherWay uses this in restrictDirection() */
|
||||
virtual void pruneInterface(InterfaceFlags accepted) {}
|
||||
/// Resolve interface/propagation direction to comply with the given external interface
|
||||
virtual void resolveInterface(InterfaceFlags /* expected */) {}
|
||||
|
||||
/// validate connectivity of children (after init() was done)
|
||||
virtual void validateConnectivity() const;
|
||||
@ -163,6 +159,7 @@ protected:
|
||||
std::string name_;
|
||||
PropertyMap properties_;
|
||||
|
||||
// pull interfaces, created by the stage as required
|
||||
InterfacePtr starts_;
|
||||
InterfacePtr ends_;
|
||||
|
||||
@ -182,6 +179,8 @@ private:
|
||||
ContainerBase* parent_; // owning parent
|
||||
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 next_starts_; // interface to be used for sendForward()
|
||||
|
||||
@ -208,19 +207,17 @@ class PropagatingEitherWayPrivate : public ComputeBasePrivate
|
||||
friend class PropagatingEitherWay;
|
||||
|
||||
public:
|
||||
PropagatingEitherWay::Direction required_interface_dirs_;
|
||||
PropagatingEitherWay::Direction configured_dir_;
|
||||
InterfaceFlags required_interface_;
|
||||
|
||||
inline PropagatingEitherWayPrivate(PropagatingEitherWay* me,
|
||||
PropagatingEitherWay::Direction required_interface_dirs_,
|
||||
inline PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction configured_dir_,
|
||||
const std::string& name);
|
||||
|
||||
InterfaceFlags requiredInterface() const override;
|
||||
// initialize pull interfaces for given propagation directions
|
||||
void initInterface(PropagatingEitherWay::Direction dir);
|
||||
// prune interface to the given propagation direction
|
||||
void pruneInterface(InterfaceFlags accepted) override;
|
||||
// validate that we can propagate in one direction at least
|
||||
void validateConnectivity() const override;
|
||||
// resolve interface to the given propagation direction
|
||||
void resolveInterface(InterfaceFlags expected) override;
|
||||
|
||||
bool canCompute() const override;
|
||||
void compute() override;
|
||||
|
||||
@ -53,10 +53,11 @@ using namespace std::placeholders;
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name) : StagePrivate(me, name) {
|
||||
pending_backward_.reset(new Interface);
|
||||
pending_forward_.reset(new Interface);
|
||||
}
|
||||
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
|
||||
: StagePrivate(me, name)
|
||||
, required_interface_(UNKNOWN)
|
||||
, pending_backward_(new Interface)
|
||||
, pending_forward_(new Interface) {}
|
||||
|
||||
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
|
||||
if (!for_insert && index < 0)
|
||||
@ -90,25 +91,9 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback& pr
|
||||
}
|
||||
|
||||
void ContainerBasePrivate::validateConnectivity() const {
|
||||
InitStageException errors;
|
||||
// recursively validate all children and accumulate errors
|
||||
for (const auto& child : children()) {
|
||||
try {
|
||||
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());
|
||||
for (const auto& child : children())
|
||||
child->pimpl()->validateConnectivity();
|
||||
}
|
||||
|
||||
bool ContainerBasePrivate::canCompute() const {
|
||||
@ -252,6 +237,11 @@ void ContainerBase::reset() {
|
||||
// ... and state mapping
|
||||
impl->internal_to_external_.clear();
|
||||
|
||||
// interfaces depend on children which might change
|
||||
impl->required_interface_ = UNKNOWN;
|
||||
impl->starts_.reset();
|
||||
impl->ends_.reset();
|
||||
|
||||
Stage::reset();
|
||||
}
|
||||
|
||||
@ -401,238 +391,103 @@ SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new
|
||||
SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name)
|
||||
: ContainerBasePrivate(me, name) {}
|
||||
|
||||
// a serial container's required interface is derived from the required input interfaces
|
||||
// of the first and last children. After resolving, it is remembered in required_interface_.
|
||||
InterfaceFlags SerialContainerPrivate::requiredInterface() const {
|
||||
if ((required_interface_ & START_IF_MASK) && (required_interface_ & END_IF_MASK))
|
||||
return required_interface_;
|
||||
void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) {
|
||||
InterfaceFlags flags1 = stage1.requiredInterface();
|
||||
InterfaceFlags flags2 = stage2.requiredInterface();
|
||||
|
||||
if (children().empty())
|
||||
return UNKNOWN;
|
||||
return (children().front()->pimpl()->requiredInterface() & START_IF_MASK) |
|
||||
(children().back()->pimpl()->requiredInterface() & END_IF_MASK);
|
||||
}
|
||||
|
||||
// connect cur stage to its predecessor and successor by setting the push interface pointers
|
||||
// return true if cur stage should be scheduled for a second sweep
|
||||
bool SerialContainerPrivate::connect(container_type::const_iterator cur) {
|
||||
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;
|
||||
}
|
||||
if ((flags1 & WRITES_NEXT_START) && (flags2 & READS_START))
|
||||
stage1.setNextStarts(stage2.starts());
|
||||
else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END))
|
||||
stage2.setPrevEnds(stage1.ends());
|
||||
else {
|
||||
boost::format desc("cannot connect end interface of '%1%' (%2%) to start interface of '%3%' (%4%)");
|
||||
desc % stage1.name() % flowSymbol<END_IF_MASK>(flags1);
|
||||
desc % stage2.name() % flowSymbol<START_IF_MASK>(flags2);
|
||||
throw InitStageException(*me(), desc.str());
|
||||
}
|
||||
// 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
|
||||
void SerialContainerPrivate::storeRequiredInterface(container_type::const_iterator first,
|
||||
container_type::const_iterator end) {
|
||||
if (first == children().begin())
|
||||
required_interface_ |= children().front()->pimpl()->interfaceFlags() & START_IF_MASK;
|
||||
if (end == children().end() && !children().empty())
|
||||
required_interface_ |= children().back()->pimpl()->interfaceFlags() & END_IF_MASK;
|
||||
template <unsigned int mask>
|
||||
void SerialContainerPrivate::validateInterface(const StagePrivate& child, InterfaceFlags required) const {
|
||||
required = required & mask;
|
||||
if (required == UNKNOWN)
|
||||
return; // cannot yet validate
|
||||
InterfaceFlags child_interface = child.interfaceFlags() & 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
|
||||
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
|
||||
// we need to have some children to do the actual work
|
||||
if (children().empty())
|
||||
return;
|
||||
throw InitStageException(*me(), "no children");
|
||||
|
||||
// reading is always allowed if current interface flags do so
|
||||
accepted |= (interfaceFlags() & InterfaceFlags({ READS_START, READS_END }));
|
||||
if (!(expected & START_IF_MASK))
|
||||
throw InitStageException(*me(), "unknown start interface");
|
||||
|
||||
if (accepted == PROPAGATE_BOTHWAYS)
|
||||
return; // There is nothing to prune
|
||||
Stage& first = *children().front();
|
||||
Stage& last = *children().back();
|
||||
|
||||
// If whole chain is still undecided, prune all children
|
||||
if (children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS &&
|
||||
children().back()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) {
|
||||
pruneInterfaces(children().begin(), children().end(), accepted);
|
||||
} else { // otherwise only prune the first / last child's input / output interface
|
||||
StagePrivate* child_impl;
|
||||
child_impl = children().front()->pimpl();
|
||||
child_impl->pruneInterface((accepted & START_IF_MASK) | (child_impl->interfaceFlags() & END_IF_MASK));
|
||||
child_impl = children().back()->pimpl();
|
||||
child_impl->pruneInterface((accepted & END_IF_MASK) | (child_impl->interfaceFlags() & START_IF_MASK));
|
||||
InitStageException exceptions;
|
||||
|
||||
try { // FIRST child
|
||||
first.pimpl()->resolveInterface(expected & START_IF_MASK);
|
||||
// connect first child's (start) push interface
|
||||
setChildsPushBackwardInterface(first.pimpl());
|
||||
// validate that first child's and this container's start interfaces match
|
||||
validateInterface<START_IF_MASK>(*first.pimpl(), expected);
|
||||
// connect first child's (start) pull interface
|
||||
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
|
||||
if (!children().front()->pimpl()->starts())
|
||||
starts_.reset();
|
||||
if (!children().back()->pimpl()->ends())
|
||||
ends_.reset();
|
||||
|
||||
if (interfaceFlags() == UNKNOWN)
|
||||
throw InitStageException(*me(), "failed to derive propagation direction");
|
||||
}
|
||||
|
||||
// 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
|
||||
// process all children and connect them
|
||||
for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) {
|
||||
try {
|
||||
StagePrivate* child_impl = (**it).pimpl();
|
||||
StagePrivate* previous_impl = (**previous_it).pimpl();
|
||||
child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
|
||||
connect(*previous_impl, *child_impl);
|
||||
} catch (InitStageException& e) {
|
||||
exceptions.append(e);
|
||||
}
|
||||
}
|
||||
|
||||
// determine accepted interface from available push interfaces
|
||||
InterfaceFlags accepted;
|
||||
|
||||
// if first stage ...
|
||||
if (first != children().begin()) {
|
||||
auto prev = first;
|
||||
--prev; // pointer to previous stage
|
||||
// ... pushes forward, we accept forward propagation
|
||||
if ((*prev)->pimpl()->requiredInterface() & WRITES_NEXT_START)
|
||||
accepted |= PROPAGATE_FORWARDS;
|
||||
// ... 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);
|
||||
try { // connect last child's (end) push interface
|
||||
setChildsPushForwardInterface(last.pimpl());
|
||||
// validate that last child's and this container's end interfaces match
|
||||
validateInterface<END_IF_MASK>(*last.pimpl(), expected);
|
||||
// connect last child's (end) pull interface
|
||||
if (const InterfacePtr& target = last.pimpl()->ends())
|
||||
ends_.reset(new Interface(
|
||||
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
|
||||
} catch (InitStageException& e) {
|
||||
exceptions.append(e);
|
||||
}
|
||||
|
||||
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 {
|
||||
InitStageException errors;
|
||||
|
||||
// recursively validate children
|
||||
try {
|
||||
ContainerBasePrivate::validateConnectivity();
|
||||
} catch (InitStageException& e) {
|
||||
errors.append(e);
|
||||
}
|
||||
ContainerBasePrivate::validateConnectivity();
|
||||
|
||||
InterfaceFlags mine = interfaceFlags();
|
||||
// check that input / output interface of first / last child matches this' resp. interface
|
||||
if (!children().empty()) {
|
||||
const StagePrivate* start = children().front()->pimpl();
|
||||
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);
|
||||
validateInterface<START_IF_MASK>(*children().front()->pimpl(), mine);
|
||||
validateInterface<END_IF_MASK>(*children().back()->pimpl(), mine);
|
||||
|
||||
const StagePrivate* last = children().back()->pimpl();
|
||||
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
|
||||
// validate connectivity of children between each other
|
||||
// ContainerBasePrivate::validateConnectivity() ensures that required push interfaces are present,
|
||||
// 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.
|
||||
@ -649,16 +504,13 @@ void SerialContainerPrivate::validateConnectivity() const {
|
||||
// start pull interface fed?
|
||||
if (cur != children().begin() && // first child has not a previous one
|
||||
(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?
|
||||
if (next != end && // last child has not a next one
|
||||
(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 {
|
||||
@ -715,74 +567,74 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti
|
||||
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
|
||||
: ContainerBasePrivate(me, name) {}
|
||||
|
||||
// A parallel container's required interface is derived from the required interfaces of all of its children.
|
||||
// They must not conflict to each other. Otherwise an InitStageException is thrown.
|
||||
InterfaceFlags ParallelContainerBasePrivate::requiredInterface() const {
|
||||
void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
|
||||
// we need to have some children to do the actual work
|
||||
if (children().empty())
|
||||
return UNKNOWN;
|
||||
/* 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)
|
||||
*/
|
||||
throw InitStageException(*me(), "no children");
|
||||
|
||||
InterfaceFlags accumulated = children().front()->pimpl()->requiredInterface();
|
||||
for (const Stage::pointer& stage : children()) {
|
||||
InterfaceFlags current = stage->pimpl()->requiredInterface();
|
||||
if (accumulated != PROPAGATE_BOTHWAYS &&
|
||||
(accumulated & current) == current) // all flags of current are already available in accumulated
|
||||
InitStageException exceptions;
|
||||
|
||||
bool first = true;
|
||||
for (const Stage::pointer& child : children()) {
|
||||
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;
|
||||
|
||||
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) {
|
||||
// forward pruning to all children with UNKNOWN required interface
|
||||
for (const Stage::pointer& stage : children()) {
|
||||
if (stage->pimpl()->requiredInterface() == UNKNOWN)
|
||||
stage->pimpl()->pruneInterface(accepted);
|
||||
void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
|
||||
bool first) const {
|
||||
const InterfaceFlags child_interface = child.requiredInterface();
|
||||
bool valid = true;
|
||||
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 {
|
||||
InitStageException errors;
|
||||
InterfaceFlags my_interface = interfaceFlags();
|
||||
InterfaceFlags children_interfaces;
|
||||
|
||||
// check that input / output interfaces of all children are handled by my interface
|
||||
for (const auto& child : children()) {
|
||||
InterfaceFlags current = child->pimpl()->interfaceFlags();
|
||||
children_interfaces |= current; // compute union of all children interfaces
|
||||
for (const auto& child : children())
|
||||
validateInterfaces(*child->pimpl(), my_interface);
|
||||
|
||||
if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK))
|
||||
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;
|
||||
ContainerBasePrivate::validateConnectivity();
|
||||
}
|
||||
|
||||
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external,
|
||||
@ -795,35 +647,6 @@ ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl)
|
||||
ParallelContainerBase::ParallelContainerBase(const std::string& 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) {
|
||||
auto impl = pimpl();
|
||||
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) {}
|
||||
|
||||
InterfaceFlags MergerPrivate::requiredInterface() const {
|
||||
if (children().size() < 2)
|
||||
throw InitStageException(*me_, "Need 2 children at least.");
|
||||
|
||||
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) {
|
||||
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
|
||||
ContainerBasePrivate::resolveInterface(expected);
|
||||
switch (interfaceFlags()) {
|
||||
case PROPAGATE_FORWARDS:
|
||||
case PROPAGATE_BACKWARDS:
|
||||
case UNKNOWN:
|
||||
break; // these are supported
|
||||
|
||||
case GENERATE:
|
||||
throw InitStageException(*me_, "Generator stages not yet supported.");
|
||||
case CONNECT:
|
||||
@ -963,7 +776,6 @@ InterfaceFlags MergerPrivate::requiredInterface() const {
|
||||
default:
|
||||
throw InitStageException(*me_, "Children's interface not supported.");
|
||||
}
|
||||
return required;
|
||||
}
|
||||
|
||||
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/introspection.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <ros/console.h>
|
||||
|
||||
#include <boost/format.hpp>
|
||||
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <algorithm>
|
||||
#include <ros/console.h>
|
||||
#include <utility>
|
||||
|
||||
namespace moveit {
|
||||
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) {
|
||||
errors_.emplace_back(std::make_pair(&stage, msg));
|
||||
}
|
||||
@ -57,7 +85,7 @@ void InitStageException::append(InitStageException& other) {
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@ -88,8 +116,12 @@ void StagePrivate::validateConnectivity() const {
|
||||
// check that the required interface is provided
|
||||
InterfaceFlags required = requiredInterface();
|
||||
InterfaceFlags actual = interfaceFlags();
|
||||
if ((required & actual) != required)
|
||||
throw InitStageException(*me(), "required interface is not satisfied");
|
||||
if ((required & actual) != required) {
|
||||
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) {
|
||||
@ -332,43 +364,6 @@ void Stage::reportPropertyError(const Property::error& e) {
|
||||
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) {
|
||||
// starts
|
||||
for (const InterfaceConstPtr& i : { impl.prevEnds(), impl.starts() }) {
|
||||
@ -379,8 +374,9 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
|
||||
os << "-";
|
||||
}
|
||||
// trajectories
|
||||
os << std::setw(5) << direction<READS_START, WRITES_PREV_END>(impl) << std::setw(3) << impl.solutions_.size()
|
||||
<< std::setw(5) << direction<READS_END, WRITES_NEXT_START>(impl);
|
||||
os << " " << flowSymbol<START_IF_MASK>(impl.interfaceFlags() | impl.requiredInterface()) << " ";
|
||||
os << std::setw(3) << impl.solutions_.size();
|
||||
os << " " << flowSymbol<END_IF_MASK>(impl.interfaceFlags() | impl.requiredInterface()) << " ";
|
||||
// ends
|
||||
for (const InterfaceConstPtr& i : { impl.ends(), impl.nextStarts() }) {
|
||||
os << std::setw(3);
|
||||
@ -398,67 +394,57 @@ ComputeBase::ComputeBase(ComputeBasePrivate* impl) : Stage(impl) {}
|
||||
|
||||
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir,
|
||||
const std::string& name)
|
||||
: ComputeBasePrivate(me, name), required_interface_dirs_(dir) {
|
||||
initInterface(required_interface_dirs_);
|
||||
: ComputeBasePrivate(me, name), configured_dir_(dir) {
|
||||
initInterface(dir);
|
||||
}
|
||||
|
||||
// initialize pull interfaces to match requested propagation directions
|
||||
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) {
|
||||
if (dir & PropagatingEitherWay::FORWARD) {
|
||||
if (!starts_) // keep existing interface if possible
|
||||
starts_.reset(
|
||||
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, std::placeholders::_1)));
|
||||
} else {
|
||||
starts_.reset();
|
||||
}
|
||||
|
||||
if (dir & PropagatingEitherWay::BACKWARD) {
|
||||
if (!ends_) // keep existing interface if possible
|
||||
ends_.reset(
|
||||
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, std::placeholders::_1)));
|
||||
} else {
|
||||
ends_.reset();
|
||||
switch (dir) {
|
||||
case PropagatingEitherWay::FORWARD:
|
||||
required_interface_ = PROPAGATE_FORWARDS;
|
||||
if (!starts_) // keep existing interface if possible
|
||||
starts_.reset(
|
||||
new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedStarts(it); }));
|
||||
ends_.reset();
|
||||
return;
|
||||
case PropagatingEitherWay::BACKWARD:
|
||||
required_interface_ = PROPAGATE_BACKWARDS;
|
||||
starts_.reset();
|
||||
if (!ends_) // keep existing interface if possible
|
||||
ends_.reset(new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedEnds(it); }));
|
||||
return;
|
||||
case PropagatingEitherWay::AUTO:
|
||||
required_interface_ = UNKNOWN;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
int dir = 0;
|
||||
if ((accepted & PROPAGATE_FORWARDS) == PROPAGATE_FORWARDS)
|
||||
dir |= PropagatingEitherWay::FORWARD;
|
||||
if ((accepted & PROPAGATE_BACKWARDS) == PROPAGATE_BACKWARDS)
|
||||
dir |= PropagatingEitherWay::BACKWARD;
|
||||
initInterface(PropagatingEitherWay::Direction(dir));
|
||||
}
|
||||
void PropagatingEitherWayPrivate::resolveInterface(InterfaceFlags expected) {
|
||||
if (expected == UNKNOWN)
|
||||
throw InitStageException(*me(), "cannot initialize to unknown interface");
|
||||
|
||||
void PropagatingEitherWayPrivate::validateConnectivity() const {
|
||||
InterfaceFlags required = requiredInterface();
|
||||
InterfaceFlags actual = interfaceFlags();
|
||||
if (actual == UNKNOWN)
|
||||
throw InitStageException(*me(), "not connected in any direction");
|
||||
|
||||
InitStageException errors;
|
||||
if ((actual & READS_START) && !(actual & WRITES_NEXT_START))
|
||||
errors.push_back(*me(), "Cannot push forwards");
|
||||
if ((actual & READS_END) && !(actual & WRITES_PREV_END))
|
||||
errors.push_back(*me(), "Cannot push backwards");
|
||||
|
||||
if (required_interface_dirs_ == PropagatingEitherWay::BOTHWAY && (required & actual) != required)
|
||||
ROS_WARN_STREAM_NAMED("PropagatingEitherWay", "Cannot propagate "
|
||||
<< (actual & PROPAGATE_FORWARDS ? "backwards" : "forwards"));
|
||||
|
||||
if (errors)
|
||||
throw errors;
|
||||
auto dir = PropagatingEitherWay::AUTO;
|
||||
if ((expected & START_IF_MASK) == READS_START || (expected & END_IF_MASK) == WRITES_NEXT_START)
|
||||
dir = PropagatingEitherWay::FORWARD;
|
||||
else if ((expected & START_IF_MASK) == WRITES_PREV_END || (expected & END_IF_MASK) == READS_END)
|
||||
dir = PropagatingEitherWay::BACKWARD;
|
||||
else {
|
||||
boost::format desc("propagator cannot satisfy expected interface %1% %2%");
|
||||
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
|
||||
throw InitStageException(*me(), desc.str());
|
||||
}
|
||||
if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) {
|
||||
boost::format desc("configured interface (%1% %2%) does not match expected one (%3% %4%)");
|
||||
desc % flowSymbol<START_IF_MASK>(required_interface_) % flowSymbol<END_IF_MASK>(required_interface_);
|
||||
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
|
||||
throw InitStageException(*me(), desc.str());
|
||||
}
|
||||
initInterface(dir);
|
||||
}
|
||||
|
||||
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
|
||||
InterfaceFlags f;
|
||||
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;
|
||||
return required_interface_;
|
||||
}
|
||||
|
||||
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
|
||||
@ -516,26 +502,12 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) :
|
||||
|
||||
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) {
|
||||
auto impl = pimpl();
|
||||
if (impl->required_interface_dirs_ == dir)
|
||||
if (impl->configured_dir_ == dir)
|
||||
return;
|
||||
if (impl->prevEnds() || impl->nextStarts())
|
||||
if (impl->configured_dir_ != AUTO)
|
||||
throw std::runtime_error("Cannot change direction after being connected");
|
||||
impl->required_interface_dirs_ = dir;
|
||||
impl->initInterface(impl->required_interface_dirs_);
|
||||
}
|
||||
|
||||
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
|
||||
impl->configured_dir_ = dir;
|
||||
impl->initInterface(dir);
|
||||
}
|
||||
|
||||
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)
|
||||
: PropagatingEitherWay(new PropagatingForwardPrivate(this, name)) {}
|
||||
|
||||
void PropagatingForward::computeBackward(const InterfaceState& to) {
|
||||
void PropagatingForward::computeBackward(const InterfaceState& /* to */) {
|
||||
assert(false); // This should never be called
|
||||
}
|
||||
|
||||
@ -568,14 +540,14 @@ PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward* me,
|
||||
PropagatingBackward::PropagatingBackward(const std::string& 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
|
||||
}
|
||||
|
||||
GeneratorPrivate::GeneratorPrivate(Generator* me, const std::string& name) : ComputeBasePrivate(me, name) {}
|
||||
|
||||
InterfaceFlags GeneratorPrivate::requiredInterface() const {
|
||||
return InterfaceFlags({ WRITES_NEXT_START, WRITES_PREV_END });
|
||||
return InterfaceFlags(GENERATE);
|
||||
}
|
||||
|
||||
bool GeneratorPrivate::canCompute() const {
|
||||
|
||||
@ -261,9 +261,7 @@ void Task::init() {
|
||||
// and *afterwards* initialize all children recursively
|
||||
stages()->init(impl->robot_model_);
|
||||
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
||||
stages()->pimpl()->pruneInterface(InterfaceFlags({ GENERATE }));
|
||||
// and *finally* validate connectivity
|
||||
stages()->pimpl()->validateConnectivity();
|
||||
stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE }));
|
||||
|
||||
// provide introspection instance to all stages
|
||||
impl->setIntrospection(impl->introspection_.get());
|
||||
|
||||
@ -11,12 +11,14 @@
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
static unsigned int mock_id = 0;
|
||||
|
||||
class GeneratorMockup : public Generator
|
||||
{
|
||||
int runs = 0;
|
||||
|
||||
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; }
|
||||
void compute() override {
|
||||
if (runs > 0)
|
||||
@ -27,10 +29,11 @@ public:
|
||||
class MonitoringGeneratorMockup : public MonitoringGenerator
|
||||
{
|
||||
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; }
|
||||
void compute() override {}
|
||||
void onNewSolution(const SolutionBase& s) override {}
|
||||
void onNewSolution(const SolutionBase&) override {}
|
||||
};
|
||||
|
||||
class PropagatorMockup : public PropagatingEitherWay
|
||||
@ -39,12 +42,13 @@ class PropagatorMockup : public PropagatingEitherWay
|
||||
int bw_runs = 0;
|
||||
|
||||
public:
|
||||
PropagatorMockup(int fw = 0, int bw = 0) : PropagatingEitherWay("either way"), fw_runs(fw), bw_runs(bw) {}
|
||||
void computeForward(const InterfaceState& from) override {
|
||||
PropagatorMockup(int fw = 0, int bw = 0)
|
||||
: PropagatingEitherWay("propagate " + std::to_string(++mock_id)), fw_runs(fw), bw_runs(bw) {}
|
||||
void computeForward(const InterfaceState& /* from */) override {
|
||||
if (fw_runs > 0)
|
||||
--fw_runs;
|
||||
}
|
||||
void computeBackward(const InterfaceState& from) override {
|
||||
void computeBackward(const InterfaceState& /* from */) override {
|
||||
if (bw_runs > 0)
|
||||
--bw_runs;
|
||||
}
|
||||
@ -54,7 +58,7 @@ class ForwardMockup : public PropagatorMockup
|
||||
public:
|
||||
ForwardMockup(int runs = 0) : PropagatorMockup(runs, 0) {
|
||||
restrictDirection(FORWARD);
|
||||
setName("forward");
|
||||
setName("forward " + std::to_string(mock_id));
|
||||
}
|
||||
};
|
||||
class BackwardMockup : public PropagatorMockup
|
||||
@ -62,7 +66,7 @@ class BackwardMockup : public PropagatorMockup
|
||||
public:
|
||||
BackwardMockup(int runs = 0) : PropagatorMockup(0, runs) {
|
||||
restrictDirection(BACKWARD);
|
||||
setName("backward");
|
||||
setName("backward " + std::to_string(mock_id));
|
||||
}
|
||||
};
|
||||
|
||||
@ -71,8 +75,8 @@ class ConnectMockup : public Connecting
|
||||
int runs = 0;
|
||||
|
||||
public:
|
||||
ConnectMockup(int runs = 0) : Connecting("connect"), runs(runs) {}
|
||||
void compute(const InterfaceState& from, const InterfaceState& to) override {
|
||||
ConnectMockup(int runs = 0) : Connecting("connect " + std::to_string(++mock_id)), runs(runs) {}
|
||||
void compute(const InterfaceState& /* from */, const InterfaceState& /* to */) override {
|
||||
if (runs > 0)
|
||||
--runs;
|
||||
}
|
||||
@ -84,32 +88,25 @@ enum StageType
|
||||
FW,
|
||||
BW,
|
||||
ANY,
|
||||
BOTH,
|
||||
CONN
|
||||
};
|
||||
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
|
||||
for (StageType t : types) {
|
||||
switch (t) {
|
||||
case GEN:
|
||||
c.insert(std::make_unique<GeneratorMockup>(runs));
|
||||
c.add(std::make_unique<GeneratorMockup>(runs));
|
||||
break;
|
||||
case FW:
|
||||
c.insert(std::make_unique<ForwardMockup>(runs));
|
||||
c.add(std::make_unique<ForwardMockup>(runs));
|
||||
break;
|
||||
case BW:
|
||||
c.insert(std::make_unique<BackwardMockup>(runs));
|
||||
c.add(std::make_unique<BackwardMockup>(runs));
|
||||
break;
|
||||
case ANY:
|
||||
c.insert(std::make_unique<PropagatorMockup>(runs, runs));
|
||||
c.add(std::make_unique<PropagatorMockup>(runs, runs));
|
||||
break;
|
||||
case BOTH: {
|
||||
auto s = std::make_unique<PropagatorMockup>(runs, runs);
|
||||
s->restrictDirection(PropagatingEitherWay::BOTHWAY);
|
||||
c.insert(std::move(s));
|
||||
break;
|
||||
}
|
||||
case CONN:
|
||||
c.insert(std::make_unique<ConnectMockup>(runs));
|
||||
c.add(std::make_unique<ConnectMockup>(runs));
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -130,13 +127,13 @@ TEST(ContainerBase, positionForInsert) {
|
||||
EXPECT_EQ(impl->childByIndex(-1, 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_EQ(impl->childByIndex(-1, true), impl->children().end());
|
||||
EXPECT_STREQ((*impl->childByIndex(-2, true))->name().c_str(), "0");
|
||||
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(1, true))->name().c_str(), "1");
|
||||
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());
|
||||
}
|
||||
|
||||
/* TODO: remove interface as it returns raw pointers */
|
||||
TEST(ContainerBase, findChild) {
|
||||
SerialContainer s, *c2;
|
||||
Stage *a, *b, *c1, *d;
|
||||
s.insert(Stage::pointer(a = new NamedStage("a")));
|
||||
s.insert(Stage::pointer(b = new NamedStage("b")));
|
||||
s.insert(Stage::pointer(c1 = new NamedStage("c")));
|
||||
auto sub = Stage::pointer(c2 = new SerialContainer("c"));
|
||||
c2->insert(Stage::pointer(d = new NamedStage("d")));
|
||||
s.insert(std::move(sub));
|
||||
s.add(Stage::pointer(a = new NamedStage("a")));
|
||||
s.add(Stage::pointer(b = new NamedStage("b")));
|
||||
s.add(Stage::pointer(c1 = new NamedStage("c")));
|
||||
auto sub = ContainerBase::pointer(c2 = new SerialContainer("c"));
|
||||
sub->add(Stage::pointer(d = new NamedStage("d")));
|
||||
s.add(std::move(sub));
|
||||
|
||||
EXPECT_EQ(s.findChild("a"), a);
|
||||
EXPECT_EQ(s.findChild("b"), b);
|
||||
@ -189,27 +187,42 @@ protected:
|
||||
impl->setPrevEnds(InterfacePtr());
|
||||
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) {
|
||||
reset(start, end);
|
||||
append(container, types);
|
||||
try {
|
||||
container.init(robot_model);
|
||||
// prune pull interfaces based on provided external interface (start, end)
|
||||
InterfaceFlags accepted;
|
||||
// resolve interfaces based on provided external interface (start, end)
|
||||
InterfaceFlags expected;
|
||||
if (start)
|
||||
accepted |= WRITES_PREV_END;
|
||||
expected |= WRITES_PREV_END;
|
||||
else
|
||||
expected |= READS_START;
|
||||
|
||||
if (end)
|
||||
accepted |= WRITES_NEXT_START;
|
||||
container.pimpl()->pruneInterface(accepted);
|
||||
expected |= WRITES_NEXT_START;
|
||||
else
|
||||
expected |= READS_END;
|
||||
|
||||
container.pimpl()->resolveInterface(expected);
|
||||
container.pimpl()->validateConnectivity();
|
||||
if (!expect_failure)
|
||||
return; // as expected
|
||||
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container;
|
||||
} catch (const InitStageException& e) {
|
||||
if (expect_failure)
|
||||
if (expect_failure) {
|
||||
std::cout << "Expected " << e << "\n" << container;
|
||||
return; // as expected
|
||||
ADD_FAILURE() << "unexpected init failure: \n" << e << "\n" << container;
|
||||
}
|
||||
ADD_FAILURE() << "Unexpected " << e << "\n" << container;
|
||||
} catch (const std::exception& e) {
|
||||
ADD_FAILURE() << "unexpected exception thrown:\n" << e.what();
|
||||
} catch (...) {
|
||||
@ -287,14 +300,14 @@ TEST_F(SerialTest, insertion_order) {
|
||||
#define EXPECT_INIT_FAILURE(start, end, ...) \
|
||||
{ \
|
||||
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
|
||||
container.clear(); \
|
||||
initContainer(); \
|
||||
validateInit(start, end, { __VA_ARGS__ }, true); \
|
||||
}
|
||||
|
||||
#define EXPECT_INIT_SUCCESS(start, end, ...) \
|
||||
{ \
|
||||
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
|
||||
container.clear(); \
|
||||
initContainer(); \
|
||||
validateInit(start, end, { __VA_ARGS__ }, false); \
|
||||
}
|
||||
|
||||
@ -310,8 +323,10 @@ TEST_F(SerialTest, init_connecting) {
|
||||
EXPECT_INIT_SUCCESS(false, false, CONN);
|
||||
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_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({ CONNECT, GENERATE }));
|
||||
|
||||
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_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
|
||||
}
|
||||
|
||||
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) {
|
||||
// container interface doesn't match children
|
||||
EXPECT_INIT_FAILURE(false, false, FW);
|
||||
@ -359,12 +401,6 @@ TEST_F(SerialTest, init_forward) {
|
||||
|
||||
EXPECT_INIT_SUCCESS(true, false, BW, ANY);
|
||||
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) {
|
||||
@ -385,6 +421,18 @@ TEST_F(SerialTest, init_backward) {
|
||||
}
|
||||
|
||||
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
|
||||
EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> ->
|
||||
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(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
|
||||
EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <-
|
||||
it = container.pimpl()->children().begin();
|
||||
@ -414,9 +456,9 @@ TEST_F(SerialTest, interface_detection) {
|
||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
||||
|
||||
// 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_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_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(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||
|
||||
EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <>
|
||||
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(true, true, ANY, ANY); // no generator
|
||||
|
||||
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- --
|
||||
it = container.pimpl()->children().begin();
|
||||
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), UNKNOWN);
|
||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), UNKNOWN);
|
||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), UNKNOWN);
|
||||
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // no connect
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
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) {
|
||||
SerialContainer* inner;
|
||||
// direction imposed from outer generator
|
||||
container.clear();
|
||||
inner = new SerialContainer("inner serial");
|
||||
append(*inner, { ANY, ANY });
|
||||
append(container, { GEN, ANY, ANY });
|
||||
container.insert(Stage::pointer(inner), -2);
|
||||
initContainer({ GEN, ANY });
|
||||
container.add(make<SerialContainer>("inner serial", { ANY, ANY }));
|
||||
append(container, { ANY });
|
||||
{
|
||||
SCOPED_TRACE("GEN - ANY - inner - ANY");
|
||||
validateInit(true, true, {}, false);
|
||||
SCOPED_TRACE("GEN - ANY - Serial( ANY - ANY ) - ANY");
|
||||
validateInit(true, true, false);
|
||||
}
|
||||
|
||||
// direction imposed from inner generator
|
||||
container.clear();
|
||||
inner = new SerialContainer("inner serial");
|
||||
append(*inner, { ANY, GEN, ANY });
|
||||
append(container, { ANY, ANY });
|
||||
container.insert(Stage::pointer(inner), -2);
|
||||
initContainer({ ANY });
|
||||
container.add(make<SerialContainer>("inner serial", { ANY, GEN, ANY }));
|
||||
append(container, { ANY });
|
||||
{
|
||||
SCOPED_TRACE("ANY - inner - ANY");
|
||||
validateInit(true, true, {}, false);
|
||||
SCOPED_TRACE("ANY - Serial( ANY - GEN - ANY ) - ANY");
|
||||
validateInit(true, true, false);
|
||||
}
|
||||
|
||||
container.clear();
|
||||
inner = new SerialContainer("inner serial");
|
||||
append(*inner, { GEN, ANY });
|
||||
container.insert(Stage::pointer(inner));
|
||||
initContainer();
|
||||
container.add(make<SerialContainer>("inner serial", { GEN, ANY }));
|
||||
append(container, { ANY });
|
||||
{
|
||||
SCOPED_TRACE("inner - ANY");
|
||||
validateInit(true, true, { ANY }, false);
|
||||
SCOPED_TRACE("Serial( GEN - ANY ) - ANY");
|
||||
validateInit(true, true, false);
|
||||
}
|
||||
|
||||
// outer and inner generators conflict with each other
|
||||
container.clear();
|
||||
inner = new SerialContainer("inner serial");
|
||||
append(*inner, { ANY, GEN, ANY });
|
||||
append(container, { GEN, ANY, ANY });
|
||||
container.insert(Stage::pointer(inner), -2);
|
||||
initContainer({ GEN, ANY });
|
||||
container.add(make<SerialContainer>("inner serial", { ANY, GEN, ANY }));
|
||||
append(container, { ANY });
|
||||
{
|
||||
SCOPED_TRACE("GEN - ANY - inner - ANY");
|
||||
validateInit(true, true, {}, true);
|
||||
SCOPED_TRACE("GEN - ANY - Serial( ANY - GEN - ANY ) - ANY");
|
||||
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>
|
||||
{};
|
||||
|
||||
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) {
|
||||
EXPECT_INIT_SUCCESS(true, true, GEN, GEN);
|
||||
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(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(true, false, 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(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(true, false, CONN, GEN);
|
||||
EXPECT_INIT_FAILURE(false, true, CONN, GEN);
|
||||
EXPECT_INIT_FAILURE(true, true, CONN, GEN);
|
||||
|
||||
EXPECT_INIT_FAILURE(false, false, BOTH, CONN);
|
||||
EXPECT_INIT_FAILURE(true, false, BOTH, CONN);
|
||||
EXPECT_INIT_FAILURE(false, true, BOTH, CONN);
|
||||
EXPECT_INIT_FAILURE(true, true, BOTH, CONN);
|
||||
EXPECT_INIT_FAILURE(false, false, BW, FW);
|
||||
EXPECT_INIT_FAILURE(true, false, BW, FW);
|
||||
EXPECT_INIT_FAILURE(false, true, BW, FW);
|
||||
EXPECT_INIT_FAILURE(true, true, BW, FW);
|
||||
}
|
||||
|
||||
EXPECT_INIT_FAILURE(false, false, BOTH, GEN);
|
||||
EXPECT_INIT_FAILURE(true, false, BOTH, GEN);
|
||||
EXPECT_INIT_FAILURE(false, true, BOTH, GEN);
|
||||
EXPECT_INIT_FAILURE(true, true, BOTH, GEN);
|
||||
TEST_F(ParallelTest, init_propagating) {
|
||||
EXPECT_INIT_SUCCESS(false, true, FW, FW);
|
||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
|
||||
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) {
|
||||
|
||||
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 FORWARD_ICON = icons::FORWARD.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();
|
||||
|
||||
if (f == InterfaceFlags(CONNECT))
|
||||
@ -133,8 +132,6 @@ QVariant BaseTaskModel::flowIcon(moveit::task_constructor::InterfaceFlags f) {
|
||||
return FORWARD_ICON;
|
||||
if (f == InterfaceFlags(PROPAGATE_BACKWARDS))
|
||||
return BACKWARD_ICON;
|
||||
if (f == PROPAGATE_BOTHWAYS)
|
||||
return BOTHWAY_ICON;
|
||||
if (f == InterfaceFlags(GENERATE))
|
||||
return GENERATE_ICON;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user