fixed remaining unittest

This commit is contained in:
Robert Haschke 2018-02-19 00:54:46 +01:00
parent 35f1540369
commit ba75f7c835
4 changed files with 50 additions and 19 deletions

View File

@ -100,10 +100,6 @@ public:
protected: protected:
ContainerBasePrivate(ContainerBase *me, const std::string &name); ContainerBasePrivate(ContainerBase *me, const std::string &name);
// containers don't required a specific interface on their own
// their interface is derived from their children
InterfaceFlags requiredInterface() const override { return InterfaceFlags(); }
// Set child's push interfaces: allow pushing if child requires it or // 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 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. // If, during pruneInterface() later, we notice that it's not needed, prune there.
@ -172,6 +168,9 @@ 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 pruneInterface(InterfaceFlags accepted) override;
@ -220,6 +219,12 @@ 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
void pruneInterface(InterfaceFlags accepted) override;
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);

View File

@ -77,6 +77,11 @@ enum InterfaceFlag {
}; };
typedef Flags<InterfaceFlag> InterfaceFlags; typedef Flags<InterfaceFlag> InterfaceFlags;
// some useful constants
constexpr InterfaceFlags UNKNOWN;
constexpr InterfaceFlags INPUT_IF_MASK({READS_START, WRITES_PREV_END});
constexpr InterfaceFlags OUTPUT_IF_MASK({READS_END, WRITES_NEXT_START});
MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(Interface)
MOVEIT_CLASS_FORWARD(Stage) MOVEIT_CLASS_FORWARD(Stage)
class InterfaceState; class InterfaceState;

View File

@ -344,11 +344,20 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
: ContainerBasePrivate(me, name) : ContainerBasePrivate(me, name)
{} {}
// a serial container's required interface is derived from the required input interface
// of the first child and the required output interface of the last child
InterfaceFlags SerialContainerPrivate::requiredInterface() const
{
if (children().empty())
return UNKNOWN;
return (children().front()->pimpl()->requiredInterface() & INPUT_IF_MASK)
| (children().back()->pimpl()->requiredInterface() & OUTPUT_IF_MASK);
}
// connect cur stage to its predecessor and successor by setting the push interface pointers // 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 // return true if cur stage should be scheduled for a second sweep
bool SerialContainerPrivate::connect(container_type::const_iterator cur) bool SerialContainerPrivate::connect(container_type::const_iterator cur)
{ {
constexpr InterfaceFlags UNKNOWN;
StagePrivate* const cur_impl = **cur; StagePrivate* const cur_impl = **cur;
InterfaceFlags required = cur_impl->requiredInterface(); InterfaceFlags required = cur_impl->requiredInterface();
@ -431,7 +440,7 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
if (children().empty()) return; if (children().empty()) return;
constexpr InterfaceFlags BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}); constexpr InterfaceFlags BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS});
// I think, we only need to deal with the special case of the whole sequence to be pruned. // We only need to deal with the special case of the whole sequence to be pruned.
if (accepted != BOTHWAYS && // will interface be restricted at all? if (accepted != BOTHWAYS && // will interface be restricted at all?
children().front()->pimpl()->interfaceFlags() == BOTHWAYS) // still undecided? children().front()->pimpl()->interfaceFlags() == BOTHWAYS) // still undecided?
{ {
@ -443,10 +452,8 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
if (!children().back()->pimpl()->ends()) if (!children().back()->pimpl()->ends())
ends_.reset(); ends_.reset();
} }
if (interfaceFlags() == UNKNOWN)
// If my assumption doesn't hold, we would need to prune partial sequences of stages throw InitStageException(*me(), "failed to derive propagation direction");
// with unknown-only interfaces at the *beginning* and *end* of the whole sequence.
// Reuse code from init() in that case.
} }
// called by init() to prune interfaces for children in range [first, last) // called by init() to prune interfaces for children in range [first, last)
@ -509,17 +516,14 @@ void SerialContainer::validateConnectivity() const
auto impl = pimpl(); auto impl = pimpl();
InitStageException errors; InitStageException errors;
constexpr InterfaceFlags INP_IF_MASK({READS_START, WRITES_PREV_END});
constexpr InterfaceFlags OUT_IF_MASK({READS_END, WRITES_NEXT_START});
// 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 (!impl->children().empty()) { if (!impl->children().empty()) {
const StagePrivate* start = impl->children().front()->pimpl(); const StagePrivate* start = impl->children().front()->pimpl();
if ((start->interfaceFlags() & INP_IF_MASK) != (this->pimpl()->interfaceFlags() & INP_IF_MASK)) if ((start->interfaceFlags() & INPUT_IF_MASK) != (this->pimpl()->interfaceFlags() & INPUT_IF_MASK))
errors.push_back(*this, "input interface of '" + start->name() + "' doesn't match mine"); errors.push_back(*this, "input interface of '" + start->name() + "' doesn't match mine");
const StagePrivate* last = impl->children().back()->pimpl(); const StagePrivate* last = impl->children().back()->pimpl();
if ((last->interfaceFlags() & OUT_IF_MASK) != (this->pimpl()->interfaceFlags() & OUT_IF_MASK)) if ((last->interfaceFlags() & OUTPUT_IF_MASK) != (this->pimpl()->interfaceFlags() & OUTPUT_IF_MASK))
errors.push_back(*this, "output interface of '" + last->name() + "' doesn't match mine"); errors.push_back(*this, "output interface of '" + last->name() + "' doesn't match mine");
} }
@ -635,6 +639,27 @@ ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase
{ {
} }
// 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
{
if (children().empty())
return UNKNOWN;
/* TODO: replace this with a proper check for consistency of interfaces. Allowed combinations are:
* = (connecting stages)
* = (backward propagating)
* = (forward propagating)
* = = = (propagating in both directions)
* = (generating)
*/
return children().front()->pimpl()->requiredInterface();
}
void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted)
{
// TODO: forward pruning to all children with UNKNOWN required interface
}
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) { void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) {
for (const Stage::pointer& stage : children()) for (const Stage::pointer& stage : children())
copyState(external, stage->pimpl()->pullInterface(dir), updated); copyState(external, stage->pimpl()->pullInterface(dir), updated);
@ -695,9 +720,6 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc
void ParallelContainerBase::validateConnectivity() const void ParallelContainerBase::validateConnectivity() const
{ {
constexpr InterfaceFlags INP_IF_MASK({READS_START, WRITES_PREV_END});
constexpr InterfaceFlags OUT_IF_MASK({READS_END, WRITES_NEXT_START});
InitStageException errors; InitStageException errors;
auto impl = pimpl(); auto impl = pimpl();
InterfaceFlags my_interface = impl->interfaceFlags(); InterfaceFlags my_interface = impl->interfaceFlags();

View File

@ -177,7 +177,6 @@ void Stage::setProperty(const std::string& name, const boost::any& value) {
template<InterfaceFlag own, InterfaceFlag other> template<InterfaceFlag own, InterfaceFlag other>
const char* direction(const StagePrivate& stage) { const char* direction(const StagePrivate& stage) {
static constexpr InterfaceFlags INPUT_IF_MASK({ READS_START | WRITES_PREV_END });
InterfaceFlags f = stage.interfaceFlags(); InterfaceFlags f = stage.interfaceFlags();
bool own_if = f & own; bool own_if = f & own;