From 87d70a5da1c5e5fbf7de3dc15aa285f54280e0b0 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 26 Mar 2020 16:24:07 +0100 Subject: [PATCH 01/13] minor documentation improvements --- core/include/moveit/task_constructor/stage_p.h | 3 ++- core/src/container.cpp | 4 ++-- core/src/stage.cpp | 4 ++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 84d0fc31..9ec47be9 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -73,7 +73,7 @@ public: /** Interface required by this stage * - * If the interface is unknown (because it is auto-detected from context), return 0 */ + * If the interface is unknown (because it is auto-detected from context), return UNKNOWN */ virtual InterfaceFlags requiredInterface() const = 0; /** Prune interface to comply with the given propagation direction @@ -163,6 +163,7 @@ protected: std::string name_; PropertyMap properties_; + // the stage's pull interfaces, created as required InterfacePtr starts_; InterfacePtr ends_; diff --git a/core/src/container.cpp b/core/src/container.cpp index f06cd894..a7a80e7d 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -105,7 +105,7 @@ void ContainerBasePrivate::validateConnectivity() const { 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%)"); + boost::format desc("%1% interface of '%2%' (%3%) does not match mine (%4%)"); errors.push_back(*me(), (desc % (mask == START_IF_MASK ? "start" : "end") % child.name() % flowSymbol(child.interfaceFlags() & mask) % flowSymbol(interfaceFlags() & mask)) .str()); @@ -948,7 +948,7 @@ InterfaceFlags MergerPrivate::requiredInterface() const { for (const Stage::pointer& stage : children()) { InterfaceFlags current = stage->pimpl()->requiredInterface(); if (current != required) - throw InitStageException(*stage, "Interface doesn't match the common one."); + throw InitStageException(*stage, "Interface does not match the common one."); } switch (required) { diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 030a876a..3ebca61e 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -57,7 +57,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; } @@ -575,7 +575,7 @@ void PropagatingBackward::computeForward(const InterfaceState& from) { 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 { From 5ae7e54732b8727c5f72a5873f9735b9fce9ba61 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 26 Mar 2020 16:25:22 +0100 Subject: [PATCH 02/13] treat empty container as init exception --- core/src/container.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index a7a80e7d..b4acd63e 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -512,7 +512,7 @@ void SerialContainerPrivate::storeRequiredInterface(container_type::const_iterat // called by parent asking for pruning of this' interface void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (children().empty()) - return; + throw InitStageException(*me(), "container is empty"); // reading is always allowed if current interface flags do so accepted |= (interfaceFlags() & InterfaceFlags({ READS_START, READS_END })); From a193fc2ec5b17c226ed3d594ef42514f92de89d4 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 3 Apr 2020 21:18:13 +0200 Subject: [PATCH 03/13] enumerate test stages remove ambiguity when multiple stages of the same type are used --- core/test/test_container.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index fb62297f..db5beb77 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -11,12 +11,14 @@ using namespace moveit::task_constructor; +static 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,7 +29,8 @@ 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 {} @@ -39,7 +42,8 @@ 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) {} + 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; @@ -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,7 +75,7 @@ class ConnectMockup : public Connecting int runs = 0; public: - ConnectMockup(int runs = 0) : Connecting("connect"), runs(runs) {} + ConnectMockup(int runs = 0) : Connecting("connect " + std::to_string(mock_id++)), runs(runs) {} void compute(const InterfaceState& from, const InterfaceState& to) override { if (runs > 0) --runs; From b6a5f89307aff1ab84ce32f43d120aec6901fe81 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 26 Mar 2020 16:27:50 +0100 Subject: [PATCH 04/13] remove logical flow BOTH / simplify pruning The PROPAGATE concept BOTH declared the stages *will* propagate solutions in either direction. ANY, on the other hand, only means the propagation direction is *not resolved yet* (but will be at planning time). BOTH was originally described to support a more general control flow than was eventually decided to support. The four exclusive Stage interfaces CONNECT, PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS, and GENERATOR do not allow for BOTH as a valid setup anymore, unless you setup a very convolved task like `Alternatives(GEN, PROP) - Alternatives(PROP, GEN)` which would be very complex to inspect. The same functionality can still be achieved more readable as `Alternatives(Seq(GEN, PROP), Seq(PROP, GEN))`. The confusion between BOTH (propagator *will* send in both directions) and ANY (propagator will send in *either* direction, decided during init) led to a lot of confusion with users and was not fully accounted throughout the pipeline. Adjust tests. Notice the difference between ANY (unresolved propagator) and UNKNOWN (a container before introspecting its children). propagators still report UNKNOWN as requiredInterface though to simplify control flow. The simplification enables a much simpler linear inference of the connective structure of a task, as the first interface direction is always given. Additionally, unify the resource setup for static interfaces to run in the constructor, and for dynamic initialization in `pruneInterface`, getting rid of partial initializations in `init`. --- .../moveit/task_constructor/container.h | 4 - .../moveit/task_constructor/container_p.h | 30 +- core/include/moveit/task_constructor/stage.h | 22 +- .../include/moveit/task_constructor/stage_p.h | 9 +- core/src/container.cpp | 379 ++++++------------ core/src/stage.cpp | 132 +++--- core/test/test_container.cpp | 255 +++++++----- .../src/task_list_model.cpp | 5 +- 8 files changed, 373 insertions(+), 463 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 538af8c2..6fd6370a 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -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); diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index ce1c7321..1c7f0188 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -105,6 +105,10 @@ public: void validateConnectivity() const override; + // containers derive their required interface from their children + // UNKNOWN until pruneInterface 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; @@ -139,6 +143,9 @@ protected: auto& internalToExternalMap() { return internal_to_external_; } const auto& internalToExternalMap() const { return internal_to_external_; } + // set by containers in pruneInterface() + InterfaceFlags required_interface_; + private: container_type children_; @@ -166,28 +173,16 @@ 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; // 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); - -private: - InterfaceFlags required_interface_; +protected: + // connect two neighbors + void connect(StagePrivate& stage1, StagePrivate& stage2); }; PIMPL_FUNCTIONS(SerialContainer) @@ -219,9 +214,6 @@ 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; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 811655df..bdffc523 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -83,13 +83,30 @@ enum InterfaceFlag PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END, GENERATE = WRITES_PREV_END | WRITES_NEXT_START, }; + typedef Flags 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; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 9ec47be9..47798af8 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -76,10 +76,11 @@ public: * If the interface is unknown (because it is auto-detected from context), return UNKNOWN */ virtual InterfaceFlags requiredInterface() const = 0; - /** Prune interface to comply with the given propagation direction - * - * PropagatingEitherWay uses this in restrictDirection() */ - virtual void pruneInterface(InterfaceFlags accepted) {} + /** validate correct interface usage and resolve ambiguous interfaces */ + virtual void pruneInterface(InterfaceFlags accepted); + + void pruneStartInterface(InterfaceFlags start_flags) { pruneInterface(start_flags & START_IF_MASK); } + void pruneEndInterface(InterfaceFlags end_flags) { pruneInterface(end_flags & END_IF_MASK); } /// validate connectivity of children (after init() was done) virtual void validateConnectivity() const; diff --git a/core/src/container.cpp b/core/src/container.cpp index b4acd63e..ab6cc0d5 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -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) @@ -252,6 +253,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,112 +407,20 @@ 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("end interface of '%1%' (%2%) does not match start interface of '%3%' (%4%)"); + desc % stage1.name() % flowSymbol(flags1 & END_IF_MASK); + desc % stage2.name() % flowSymbol(flags2 & START_IF_MASK); + 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; } // called by parent asking for pruning of this' interface @@ -514,98 +428,50 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (children().empty()) throw InitStageException(*me(), "container is empty"); - // reading is always allowed if current interface flags do so - accepted |= (interfaceFlags() & InterfaceFlags({ READS_START, READS_END })); + // TODO(v4hn): if ever there is a use case to start pruning + // with a specified end interface, this would need to be extended + if (!(accepted & (READS_START | WRITES_PREV_END))) + return; // The start interface direction is not decided - 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)); + // sweep through children once: infer and connect interfaces + first.pimpl()->pruneStartInterface(accepted); + setChildsPushBackwardInterface(first); + + for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) { + StagePrivate* child_impl = (**it).pimpl(); + StagePrivate* previous_impl = (**previous_it).pimpl(); + child_impl->pruneStartInterface(invert(previous_impl->requiredInterface())); + connect(*previous_impl, *child_impl); } - // reset my pull interfaces, if first/last child don't pull anymore - if (!children().front()->pimpl()->starts()) + // potentially connect outmost push interface to pending_ buffer + setChildsPushForwardInterface(last); + + if ((accepted & END_IF_MASK) != UNKNOWN && + (last.pimpl()->requiredInterface() & END_IF_MASK) != (accepted & END_IF_MASK)) { + boost::format desc( + "requested end interface for container (%1%) does not agree with inferred end interface of last child (%2%)"); + desc % flowSymbol(accepted & END_IF_MASK) % flowSymbol(last.pimpl()->requiredInterface() & END_IF_MASK); + throw InitStageException(*me(), desc.str()); + } + + // if first/last pull, this needs to pull to and forward to the children + if (const InterfacePtr& target = first.pimpl()->starts()) + starts_.reset(new Interface( + [this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); })); + else starts_.reset(); - if (!children().back()->pimpl()->ends()) + + if (const InterfacePtr& target = last.pimpl()->ends()) + ends_.reset(new Interface( + [this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); })); + else 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 - } - - // 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); - } - - storeRequiredInterface(first, end); + required_interface_ = first.pimpl()->interfaceFlags() & START_IF_MASK | last.pimpl()->interfaceFlags() & END_IF_MASK; } void SerialContainerPrivate::validateConnectivity() const { @@ -715,64 +581,78 @@ 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 { - 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) - */ - - 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 - 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; -} - 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); + if (children().empty()) + throw InitStageException(*me(), "trying to prune empty container"); + + if (accepted == UNKNOWN) + return; // nothing to prune + + InitStageException exceptions; + InterfaceFlags interface; + + for (const Stage::pointer& child : children()) { + try { + child->pimpl()->pruneInterface(accepted); + } catch (InitStageException& e) { + exceptions.append(e); + continue; + } + + InterfaceFlags child_interface = child->pimpl()->requiredInterface(); + if (interface == UNKNOWN) + interface = child_interface; + else if ((interface & child_interface) != child_interface) { + boost::format desc("inferred interface of stage '%1%' (%2%/%3%) does not agree with the inferred interface of " + "its siblings (%4%/%5%)."); + desc % child->name(); + desc % flowSymbol(child_interface & START_IF_MASK) % flowSymbol(child_interface & END_IF_MASK); + desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK); + exceptions.push_back(*me(), desc.str()); + } } + + if ((interface & accepted) != accepted) { + boost::format desc("required interface (%1%/%2%) does not match children (%3%/%4%)."); + desc % flowSymbol(accepted & START_IF_MASK) % flowSymbol(accepted & END_IF_MASK); + desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK); + exceptions.push_back(*me(), desc.str()); + } + + if (exceptions) + throw exceptions; + + // States received by the container need to be copied to all children's pull interfaces. + if (interface & READS_START) + starts().reset(new Interface([this](Interface::iterator external, bool updated) { + this->onNewExternalState(Interface::FORWARD, external, updated); + })); + if (interface & READS_END) + ends().reset(new Interface([this](Interface::iterator external, bool updated) { + this->onNewExternalState(Interface::BACKWARD, external, updated); + })); + + // initialize push connections of children according to their demands + for (const Stage::pointer& stage : children()) { + setChildsPushForwardInterface(*stage); + setChildsPushBackwardInterface(*stage); + } + + required_interface_ = interface; } 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 - 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 { @@ -795,35 +675,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(impl, &solution, cost, std::move(comment)), solution.start(), diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 3ebca61e..7e315ff3 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -39,10 +39,14 @@ #include #include #include + +#include + +#include + #include #include #include -#include #include namespace moveit { @@ -84,12 +88,38 @@ InterfaceFlags StagePrivate::interfaceFlags() const { return f; } +void StagePrivate::pruneInterface(InterfaceFlags accepted) { + const InterfaceFlags accepted_start{ accepted & START_IF_MASK }; + const InterfaceFlags accepted_end{ accepted & END_IF_MASK }; + + const InterfaceFlags required{ requiredInterface() }; + const InterfaceFlags required_start{ required & START_IF_MASK }; + const InterfaceFlags required_end{ required & END_IF_MASK }; + + if (required == UNKNOWN) + throw std::logic_error("stage type does not specify interface, but also does not override pruneInterface. " + "Who sets up its interfaces?"); + + // only one side needs to be specified but the value has to be compatible with this stage + if (((accepted_start != UNKNOWN) && (accepted_start & required_start) != required_start) || + ((accepted_end != UNKNOWN) && (accepted_end & required_end) != required_end)) { + boost::format desc("this' interface %1%/%2% cannot match inferred interface %3%/%4%"); + desc % flowSymbol(required_start) % flowSymbol(required_end); + desc % flowSymbol(accepted_start) % flowSymbol(accepted_end); + throw InitStageException(*me(), desc.str()); + } +} + 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("interface %1%/%2% does not satisfy required interface %3%/%4%"); + desc % flowSymbol(actual & START_IF_MASK) % flowSymbol(actual & END_IF_MASK); + desc % flowSymbol(required & START_IF_MASK) % flowSymbol(required & END_IF_MASK); + throw InitStageException(*me(), desc.str()); + } } bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { @@ -398,40 +428,50 @@ 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), required_interface_dirs_(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 (required_interface_dirs_ != PropagatingEitherWay::AUTO && dir != required_interface_dirs_) { + auto flow = [](PropagatingEitherWay::Direction dir) -> const char* { + switch (dir) { + case PropagatingEitherWay::AUTO: + return flowSymbol(InterfaceFlags{ READS_START, WRITES_PREV_END }); + case PropagatingEitherWay::FORWARD: + return flowSymbol(InterfaceFlags{ READS_START }); + case PropagatingEitherWay::BACKWARD: + return flowSymbol(InterfaceFlags{ WRITES_PREV_END }); + } + }; + + boost::format desc("propagation direction %1% incompatible with inferred direction %2%"); + desc % flow(required_interface_dirs_) % flow(dir); + throw InitStageException(*me(), desc.str()); } - 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(); - } + if (dir == PropagatingEitherWay::FORWARD) + starts_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedStarts(it); })); + else if (dir == PropagatingEitherWay::BACKWARD) + ends_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedEnds(it); })); + + required_interface_dirs_ = dir; } 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)); + if (accepted == UNKNOWN) + throw InitStageException(*me(), std::string("cannot prune to ") + flowSymbol(UNKNOWN)); + if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START) + initInterface(PropagatingEitherWay::FORWARD); + else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END) + initInterface(PropagatingEitherWay::BACKWARD); + else { + boost::format desc("propagator cannot act with interfaces start(%1%), end(%2%)"); + desc % flowSymbol(accepted & START_IF_MASK) % flowSymbol(accepted & END_IF_MASK); + throw InitStageException(*me(), desc.str()); + } } void PropagatingEitherWayPrivate::validateConnectivity() const { - InterfaceFlags required = requiredInterface(); InterfaceFlags actual = interfaceFlags(); if (actual == UNKNOWN) throw InitStageException(*me(), "not connected in any direction"); @@ -442,23 +482,19 @@ void PropagatingEitherWayPrivate::validateConnectivity() const { 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; } 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; + switch (required_interface_dirs_) { + case PropagatingEitherWay::FORWARD: + return PROPAGATE_FORWARDS; + case PropagatingEitherWay::BACKWARD: + return PROPAGATE_BACKWARDS; + case PropagatingEitherWay::AUTO: + return UNKNOWN; + } } void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) { @@ -516,26 +552,10 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) : void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) { auto impl = pimpl(); - if (impl->required_interface_dirs_ == dir) - return; - if (impl->prevEnds() || impl->nextStarts()) + if (impl->required_interface_dirs_ != 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->initInterface(dir); } void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) { diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index db5beb77..717f5d02 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -88,32 +88,25 @@ enum StageType FW, BW, ANY, - BOTH, CONN }; void append(ContainerBase& c, const std::initializer_list& types, int runs = 0) { for (StageType t : types) { switch (t) { case GEN: - c.insert(std::make_unique(runs)); + c.add(std::make_unique(runs)); break; case FW: - c.insert(std::make_unique(runs)); + c.add(std::make_unique(runs)); break; case BW: - c.insert(std::make_unique(runs)); + c.add(std::make_unique(runs)); break; case ANY: - c.insert(std::make_unique(runs, runs)); + c.add(std::make_unique(runs, runs)); break; - case BOTH: { - auto s = std::make_unique(runs, runs); - s->restrictDirection(PropagatingEitherWay::BOTHWAY); - c.insert(std::move(s)); - break; - } case CONN: - c.insert(std::make_unique(runs)); + c.add(std::make_unique(runs)); break; } } @@ -134,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("0")); + s.add(std::make_unique("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("1")); + s.add(std::make_unique("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()); @@ -151,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); @@ -194,17 +188,25 @@ protected: pushInterface(start, end); } + void validateInit(bool start, bool end, bool expect_failure) { validateInit(start, end, {}, expect_failure); } + void validateInit(bool start, bool end, const std::initializer_list& 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) + // prune interfaces based on provided external interface (start, end) InterfaceFlags accepted; if (start) accepted |= WRITES_PREV_END; + else + accepted |= READS_START; + if (end) accepted |= WRITES_NEXT_START; + else + accepted |= READS_END; + container.pimpl()->pruneInterface(accepted); container.pimpl()->validateConnectivity(); if (!expect_failure) @@ -314,8 +316,7 @@ TEST_F(SerialTest, init_connecting) { EXPECT_INIT_SUCCESS(false, false, CONN); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); - EXPECT_INIT_FAILURE(true, true, CONN); - EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({ CONNECT, GENERATE })); + EXPECT_INIT_FAILURE(true, true, CONN); // external interface says GENERATOR, stage is CONNECTOR EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected } @@ -363,12 +364,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) { @@ -389,6 +384,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(); @@ -397,12 +404,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(); @@ -435,109 +436,94 @@ 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 } 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); + + append(container, { GEN, ANY }); + container.add(Stage::pointer(inner)); + 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); + + append(container, { ANY }); + container.add(Stage::pointer(inner)); + 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)); + 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); + + append(container, { GEN, ANY }); + container.add(Stage::pointer(inner)); + 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) { + container.clear(); + + append(container, { GEN }); + auto inner_alt = new Alternatives("inner alternatives"); + append(*inner_alt, { ANY, ANY }); + container.add(Stage::pointer(inner_alt)); + { + SCOPED_TRACE("GEN - Alternatives( ANY - ANY )"); + validateInit(true, true, false); + } + + container.clear(); + append(container, { GEN }); + auto inner_fb = new Fallbacks("inner alternatives"); + append(*inner_fb, { ANY, FW }); + container.add(Stage::pointer(inner_fb)); + { + SCOPED_TRACE("GEN - Fallback( ANY - FW )"); + validateInit(true, true, false); } } class ParallelTest : public InitTest {}; -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); @@ -566,6 +552,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); @@ -576,20 +567,66 @@ 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); +TEST_F(ParallelTest, init_propagating) { + EXPECT_INIT_SUCCESS(false, true, FW, FW); + EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS); - 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); + 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); + + EXPECT_INIT_FAILURE(false, false, FW, FW); + EXPECT_INIT_FAILURE(true, false, FW, FW); + EXPECT_INIT_FAILURE(true, true, FW, FW); + + // conflicting definitions + EXPECT_INIT_FAILURE(true, true, BW, FW); // no generator + EXPECT_INIT_FAILURE(false, false, BW, FW); // no connect + + // The following two cases could in principle be allowed, + // assuming the non-matching children are plain disabled, + // but that behavior leads to very frustrating troubleshooting + EXPECT_INIT_FAILURE(false, true, BW, FW); + EXPECT_INIT_FAILURE(true, false, BW, FW); +} + +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)); + auto it = container.pimpl()->children().cbegin(); + EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); + EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); + + EXPECT_INIT_SUCCESS(true, false, ANY, ANY); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); + it = container.pimpl()->children().cbegin(); + EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); + EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); + + EXPECT_INIT_SUCCESS(true, false, BW, ANY); // infer ANY as BACKWARDS + EXPECT_EQ((*container.pimpl()->childByIndex(1))->pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS); + EXPECT_INIT_SUCCESS(false, true, ANY, FW); // infer ANY as FORWARDS + EXPECT_EQ((*container.pimpl()->childByIndex(0))->pimpl()->interfaceFlags(), PROPAGATE_FORWARDS); + EXPECT_INIT_SUCCESS(false, true, FW, ANY, FW); // infer ANY as FORWARDS + EXPECT_EQ((*container.pimpl()->childByIndex(1))->pimpl()->interfaceFlags(), PROPAGATE_FORWARDS); } TEST(Task, move) { diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index c505e315..55c41fb7 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -124,8 +124,9 @@ 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(); + // might be needed for graphical setup of tasks + // static const QIcon BOTHWAY_ICON = icons::BOTHWAY.icon(); if (f == InterfaceFlags(CONNECT)) return CONNECT_ICON; @@ -133,8 +134,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; From 7f33633e828070949c59a4f9f4d46940c04cc8d5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 8 Apr 2020 20:58:14 +0200 Subject: [PATCH 05/13] Simplify test_container.cpp * Reset mock_id for each test to facilitate assignment * Simplify creation of nested containers * Moved some test cases around * Output all exception messages (even expected ones) --- core/test/test_container.cpp | 155 ++++++++++++++++------------------- 1 file changed, 72 insertions(+), 83 deletions(-) diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 717f5d02..a8738836 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -11,14 +11,14 @@ using namespace moveit::task_constructor; -static int mock_id = 0; +static unsigned int mock_id = 0; class GeneratorMockup : public Generator { int runs = 0; public: - GeneratorMockup(int runs = 0) : Generator("generator " + std::to_string(mock_id++)), 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) @@ -30,10 +30,10 @@ class MonitoringGeneratorMockup : public MonitoringGenerator { public: MonitoringGeneratorMockup(Stage* monitored) - : MonitoringGenerator("monitoring generator " + std::to_string(mock_id++), 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 @@ -43,12 +43,12 @@ class PropagatorMockup : public PropagatingEitherWay public: 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 { + : 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; } @@ -58,7 +58,7 @@ class ForwardMockup : public PropagatorMockup public: ForwardMockup(int runs = 0) : PropagatorMockup(runs, 0) { restrictDirection(FORWARD); - setName("forward " + std::to_string(mock_id++)); + setName("forward " + std::to_string(mock_id)); } }; class BackwardMockup : public PropagatorMockup @@ -66,7 +66,7 @@ class BackwardMockup : public PropagatorMockup public: BackwardMockup(int runs = 0) : PropagatorMockup(0, runs) { restrictDirection(BACKWARD); - setName("backward " + std::to_string(mock_id++)); + setName("backward " + std::to_string(mock_id)); } }; @@ -75,8 +75,8 @@ class ConnectMockup : public Connecting int runs = 0; public: - ConnectMockup(int runs = 0) : Connecting("connect " + std::to_string(mock_id++)), 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; } @@ -187,6 +187,11 @@ protected: impl->setPrevEnds(InterfacePtr()); pushInterface(start, end); } + void initContainer(const std::initializer_list& types = {}) { + container.clear(); + mock_id = 0; + append(container, types); + } void validateInit(bool start, bool end, bool expect_failure) { validateInit(start, end, {}, expect_failure); } @@ -213,9 +218,11 @@ protected: 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 (...) { @@ -293,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); \ } @@ -316,7 +323,10 @@ TEST_F(SerialTest, init_connecting) { EXPECT_INIT_SUCCESS(false, false, CONN); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); - EXPECT_INIT_FAILURE(true, true, CONN); // external interface says GENERATOR, stage is CONNECTOR + // external interface does not match CONNECT + EXPECT_INIT_FAILURE(false, true, CONN); + EXPECT_INIT_FAILURE(true, false, CONN); + EXPECT_INIT_FAILURE(true, true, CONN); EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected } @@ -325,7 +335,10 @@ 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 } @@ -441,17 +454,17 @@ TEST_F(SerialTest, interface_detection) { EXPECT_INIT_FAILURE(false, false, ANY, ANY); // no connect } +template +Stage::pointer make(const std::string& name, const std::initializer_list& 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 }); - container.add(Stage::pointer(inner)); + initContainer({ GEN, ANY }); + container.add(make("inner serial", { ANY, ANY })); append(container, { ANY }); { SCOPED_TRACE("GEN - ANY - Serial( ANY - ANY ) - ANY"); @@ -459,24 +472,16 @@ TEST_F(SerialTest, nested_interface_detection) { } // direction imposed from inner generator - container.clear(); - - inner = new SerialContainer("inner serial"); - append(*inner, { ANY, GEN, ANY }); - - append(container, { ANY }); - container.add(Stage::pointer(inner)); + initContainer({ ANY }); + container.add(make("inner serial", { ANY, GEN, ANY })); append(container, { ANY }); { 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("inner serial", { GEN, ANY })); append(container, { ANY }); { SCOPED_TRACE("Serial( GEN - ANY ) - ANY"); @@ -484,37 +489,25 @@ TEST_F(SerialTest, nested_interface_detection) { } // outer and inner generators conflict with each other - container.clear(); - - inner = new SerialContainer("inner serial"); - append(*inner, { ANY, GEN, ANY }); - - append(container, { GEN, ANY }); - container.add(Stage::pointer(inner)); + initContainer({ GEN, ANY }); + container.add(make("inner serial", { ANY, GEN, ANY })); append(container, { ANY }); { SCOPED_TRACE("GEN - ANY - Serial( ANY - GEN - ANY ) - ANY"); - validateInit(true, true, {}, true); // expected to fail + validateInit(true, true, true); // expected to fail } } TEST_F(SerialTest, nested_parallel) { - container.clear(); - - append(container, { GEN }); - auto inner_alt = new Alternatives("inner alternatives"); - append(*inner_alt, { ANY, ANY }); - container.add(Stage::pointer(inner_alt)); + initContainer({ GEN }); + container.add(make("inner alternatives", { ANY, ANY })); { SCOPED_TRACE("GEN - Alternatives( ANY - ANY )"); validateInit(true, true, false); } - container.clear(); - append(container, { GEN }); - auto inner_fb = new Fallbacks("inner alternatives"); - append(*inner_fb, { ANY, FW }); - container.add(Stage::pointer(inner_fb)); + initContainer({ GEN }); + container.add(make("inner alternatives", { ANY, FW })); { SCOPED_TRACE("GEN - Fallback( ANY - FW )"); validateInit(true, true, false); @@ -576,32 +569,25 @@ TEST_F(ParallelTest, init_mismatching) { 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, BW, FW); + EXPECT_INIT_FAILURE(true, false, BW, FW); + EXPECT_INIT_FAILURE(false, true, BW, FW); + EXPECT_INIT_FAILURE(true, true, BW, FW); } TEST_F(ParallelTest, init_propagating) { 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); - - 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); - // conflicting definitions - EXPECT_INIT_FAILURE(true, true, BW, FW); // no generator - EXPECT_INIT_FAILURE(false, false, BW, FW); // no connect - - // The following two cases could in principle be allowed, - // assuming the non-matching children are plain disabled, - // but that behavior leads to very frustrating troubleshooting - EXPECT_INIT_FAILURE(false, true, BW, FW); - EXPECT_INIT_FAILURE(true, false, BW, 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) { @@ -611,22 +597,25 @@ TEST_F(ParallelTest, init_any) { // infer container (and children) direction from outside EXPECT_INIT_SUCCESS(false, true, ANY, ANY); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); - auto it = container.pimpl()->children().cbegin(); - EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); - EXPECT_EQ((*++it)->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)); - it = container.pimpl()->children().cbegin(); - EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); - EXPECT_EQ((*++it)->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 - EXPECT_EQ((*container.pimpl()->childByIndex(1))->pimpl()->interfaceFlags(), PROPAGATE_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 - EXPECT_EQ((*container.pimpl()->childByIndex(0))->pimpl()->interfaceFlags(), PROPAGATE_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 - EXPECT_EQ((*container.pimpl()->childByIndex(1))->pimpl()->interfaceFlags(), PROPAGATE_FORWARDS); + for (const auto& child : container.pimpl()->children()) + EXPECT_EQ(child->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)) << child->name(); } TEST(Task, move) { From a98d5ed8ae939689a9bb6bf5f2f12f4813e46c3b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Apr 2020 13:20:18 +0200 Subject: [PATCH 06/13] templated flowSymbol(flags) * Enforce at compile time that either input or output flags are considered. * Use horizontal flow symbols (as in console output). * Replace redundant direction(flags). --- .../moveit/task_constructor/container_p.h | 3 +- core/include/moveit/task_constructor/stage.h | 4 +- core/src/container.cpp | 28 +++--- core/src/stage.cpp | 88 ++++++++----------- 4 files changed, 57 insertions(+), 66 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 1c7f0188..45858028 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -133,7 +133,8 @@ protected: child.pimpl()->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; + template + void mismatchingInterface(InitStageException& errors, const StagePrivate& child) 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); diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index bdffc523..6961e1a9 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -382,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 +const char* flowSymbol(InterfaceFlags f); } } diff --git a/core/src/container.cpp b/core/src/container.cpp index ab6cc0d5..a21e83b5 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -104,11 +104,11 @@ void ContainerBasePrivate::validateConnectivity() const { throw errors; } -void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child, - const InterfaceFlags mask) const { +template +void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child) const { boost::format desc("%1% interface of '%2%' (%3%) does not match mine (%4%)"); errors.push_back(*me(), (desc % (mask == START_IF_MASK ? "start" : "end") % child.name() % - flowSymbol(child.interfaceFlags() & mask) % flowSymbol(interfaceFlags() & mask)) + flowSymbol(child.interfaceFlags()) % flowSymbol(interfaceFlags())) .str()); } @@ -417,8 +417,8 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) stage2.setPrevEnds(stage1.ends()); else { boost::format desc("end interface of '%1%' (%2%) does not match start interface of '%3%' (%4%)"); - desc % stage1.name() % flowSymbol(flags1 & END_IF_MASK); - desc % stage2.name() % flowSymbol(flags2 & START_IF_MASK); + desc % stage1.name() % flowSymbol(flags1); + desc % stage2.name() % flowSymbol(flags2); throw InitStageException(*me(), desc.str()); } } @@ -454,7 +454,7 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { (last.pimpl()->requiredInterface() & END_IF_MASK) != (accepted & END_IF_MASK)) { boost::format desc( "requested end interface for container (%1%) does not agree with inferred end interface of last child (%2%)"); - desc % flowSymbol(accepted & END_IF_MASK) % flowSymbol(last.pimpl()->requiredInterface() & END_IF_MASK); + desc % flowSymbol(accepted) % flowSymbol(last.pimpl()->requiredInterface()); throw InitStageException(*me(), desc.str()); } @@ -490,12 +490,12 @@ void SerialContainerPrivate::validateConnectivity() const { 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); + mismatchingInterface(errors, *start); 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); + mismatchingInterface(errors, *last); } // validate connectivity of children amongst each other @@ -606,16 +606,16 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) { boost::format desc("inferred interface of stage '%1%' (%2%/%3%) does not agree with the inferred interface of " "its siblings (%4%/%5%)."); desc % child->name(); - desc % flowSymbol(child_interface & START_IF_MASK) % flowSymbol(child_interface & END_IF_MASK); - desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK); + desc % flowSymbol(child_interface) % flowSymbol(child_interface); + desc % flowSymbol(interface) % flowSymbol(interface); exceptions.push_back(*me(), desc.str()); } } if ((interface & accepted) != accepted) { boost::format desc("required interface (%1%/%2%) does not match children (%3%/%4%)."); - desc % flowSymbol(accepted & START_IF_MASK) % flowSymbol(accepted & END_IF_MASK); - desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK); + desc % flowSymbol(accepted) % flowSymbol(accepted); + desc % flowSymbol(interface) % flowSymbol(interface); exceptions.push_back(*me(), desc.str()); } @@ -649,9 +649,9 @@ void ParallelContainerBasePrivate::validateConnectivity() const { for (const auto& child : children()) { InterfaceFlags current = child->pimpl()->interfaceFlags(); if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK)) - mismatchingInterface(errors, *child->pimpl(), START_IF_MASK); + mismatchingInterface(errors, *child->pimpl()); if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK)) - mismatchingInterface(errors, *child->pimpl(), END_IF_MASK); + mismatchingInterface(errors, *child->pimpl()); } // recursively validate children diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 7e315ff3..c48186db 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -52,6 +52,30 @@ namespace moveit { namespace task_constructor { +template <> +const char* flowSymbol(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(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)); } @@ -103,9 +127,9 @@ void StagePrivate::pruneInterface(InterfaceFlags accepted) { // only one side needs to be specified but the value has to be compatible with this stage if (((accepted_start != UNKNOWN) && (accepted_start & required_start) != required_start) || ((accepted_end != UNKNOWN) && (accepted_end & required_end) != required_end)) { - boost::format desc("this' interface %1%/%2% cannot match inferred interface %3%/%4%"); - desc % flowSymbol(required_start) % flowSymbol(required_end); - desc % flowSymbol(accepted_start) % flowSymbol(accepted_end); + boost::format desc("interface %1% %2% does not match inferred interface %3% %4%"); + desc % flowSymbol(required_start) % flowSymbol(required_end); + desc % flowSymbol(accepted_start) % flowSymbol(accepted_end); throw InitStageException(*me(), desc.str()); } } @@ -115,9 +139,9 @@ void StagePrivate::validateConnectivity() const { InterfaceFlags required = requiredInterface(); InterfaceFlags actual = interfaceFlags(); if ((required & actual) != required) { - boost::format desc("interface %1%/%2% does not satisfy required interface %3%/%4%"); - desc % flowSymbol(actual & START_IF_MASK) % flowSymbol(actual & END_IF_MASK); - desc % flowSymbol(required & START_IF_MASK) % flowSymbol(required & END_IF_MASK); + boost::format desc("interface %1% %2% does not satisfy required interface %3% %4%"); + desc % flowSymbol(actual) % flowSymbol(actual); + desc % flowSymbol(required) % flowSymbol(required); throw InitStageException(*me(), desc.str()); } } @@ -362,43 +386,6 @@ void Stage::reportPropertyError(const Property::error& e) { throw std::runtime_error(oss.str()); } -template -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(f & START_IF_MASK) ^ static_cast(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() }) { @@ -409,8 +396,9 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { os << "-"; } // trajectories - os << std::setw(5) << direction(impl) << std::setw(3) << impl.solutions_.size() - << std::setw(5) << direction(impl); + os << " " << flowSymbol(impl.interfaceFlags() | impl.requiredInterface()) << " "; + os << std::setw(3) << impl.solutions_.size(); + os << " " << flowSymbol(impl.interfaceFlags() | impl.requiredInterface()) << " "; // ends for (const InterfaceConstPtr& i : { impl.ends(), impl.nextStarts() }) { os << std::setw(3); @@ -436,11 +424,11 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction auto flow = [](PropagatingEitherWay::Direction dir) -> const char* { switch (dir) { case PropagatingEitherWay::AUTO: - return flowSymbol(InterfaceFlags{ READS_START, WRITES_PREV_END }); + return flowSymbol(InterfaceFlags{ READS_START, WRITES_PREV_END }); case PropagatingEitherWay::FORWARD: - return flowSymbol(InterfaceFlags{ READS_START }); + return flowSymbol(InterfaceFlags{ READS_START }); case PropagatingEitherWay::BACKWARD: - return flowSymbol(InterfaceFlags{ WRITES_PREV_END }); + return flowSymbol(InterfaceFlags{ WRITES_PREV_END }); } }; @@ -459,14 +447,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) { if (accepted == UNKNOWN) - throw InitStageException(*me(), std::string("cannot prune to ") + flowSymbol(UNKNOWN)); + throw InitStageException(*me(), "cannot prune to unknown interface"); if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START) initInterface(PropagatingEitherWay::FORWARD); else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END) initInterface(PropagatingEitherWay::BACKWARD); else { boost::format desc("propagator cannot act with interfaces start(%1%), end(%2%)"); - desc % flowSymbol(accepted & START_IF_MASK) % flowSymbol(accepted & END_IF_MASK); + desc % flowSymbol(accepted) % flowSymbol(accepted); throw InitStageException(*me(), desc.str()); } } From 9116e92d08337c4d541e0650ff2773a990bd730f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Apr 2020 13:45:00 +0200 Subject: [PATCH 07/13] minor cleanup, fixing warnings, improving comments --- core/include/moveit/task_constructor/stage_p.h | 4 +++- core/src/stage.cpp | 4 ++-- visualization/motion_planning_tasks/src/task_list_model.cpp | 2 -- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 47798af8..e2386267 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -164,7 +164,7 @@ protected: std::string name_; PropertyMap properties_; - // the stage's pull interfaces, created as required + // pull interfaces, created by the stage as required InterfacePtr starts_; InterfacePtr ends_; @@ -184,6 +184,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() diff --git a/core/src/stage.cpp b/core/src/stage.cpp index c48186db..3a65f9a2 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -563,7 +563,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 } @@ -576,7 +576,7 @@ 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 } diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 55c41fb7..31a57a3d 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -125,8 +125,6 @@ QVariant BaseTaskModel::flowIcon(moveit::task_constructor::InterfaceFlags f) { static const QIcon FORWARD_ICON = icons::FORWARD.icon(); static const QIcon BACKWARD_ICON = icons::BACKWARD.icon(); static const QIcon GENERATE_ICON = icons::GENERATE.icon(); - // might be needed for graphical setup of tasks - // static const QIcon BOTHWAY_ICON = icons::BOTHWAY.icon(); if (f == InterfaceFlags(CONNECT)) return CONNECT_ICON; From 4d8d32dda5dd09103dccf96c43b2889124442476 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Apr 2020 23:25:59 +0200 Subject: [PATCH 08/13] New test: keep previously configured propagate interface --- core/test/test_container.cpp | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index a8738836..b6853104 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -343,6 +343,30 @@ TEST_F(SerialTest, init_generator) { 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); From 10260b9dd7f9d2f8e39941e160fba7ffa5db4530 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Apr 2020 17:26:48 +0200 Subject: [PATCH 09/13] Further simplify interface resolution * Do not modify explicitly configured direction of Propagator * Avoid code duplication in pruneInterface() and validateConnectivity() Only implement pruneInterface() for stages that actually need to adapt their interface. --- .../moveit/task_constructor/container_p.h | 10 +- .../include/moveit/task_constructor/stage_p.h | 15 ++- core/src/container.cpp | 75 +++++--------- core/src/stage.cpp | 97 ++++++++----------- 4 files changed, 75 insertions(+), 122 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 45858028..af204e00 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -105,7 +105,7 @@ public: void validateConnectivity() const override; - // containers derive their required interface from their children + // Containers derive their required interface from their children // UNKNOWN until pruneInterface was called InterfaceFlags requiredInterface() const override { return required_interface_; } @@ -124,12 +124,12 @@ protected: // 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); + bool allowed = (required & WRITES_PREV_END); child.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr()); } inline void setChildsPushForwardInterface(Stage& child) { InterfaceFlags required = child.pimpl()->requiredInterface(); - bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN); + bool allowed = (required & WRITES_NEXT_START); child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } // report error about mismatching interface (start or end as determined by mask) @@ -144,7 +144,7 @@ protected: auto& internalToExternalMap() { return internal_to_external_; } const auto& internalToExternalMap() const { return internal_to_external_; } - // set by containers in pruneInterface() + // set in pruneInterface() InterfaceFlags required_interface_; private: @@ -249,7 +249,7 @@ public: typedef std::function Spawner; MergerPrivate(Merger* me, const std::string& name); - InterfaceFlags requiredInterface() const override; + void pruneInterface(InterfaceFlags accepted) override; void onNewPropagateSolution(const SolutionBase& s); void onNewGeneratorSolution(const SolutionBase& s); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index e2386267..572bc2d0 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -76,11 +76,10 @@ public: * If the interface is unknown (because it is auto-detected from context), return UNKNOWN */ virtual InterfaceFlags requiredInterface() const = 0; - /** validate correct interface usage and resolve ambiguous interfaces */ - virtual void pruneInterface(InterfaceFlags accepted); - - void pruneStartInterface(InterfaceFlags start_flags) { pruneInterface(start_flags & START_IF_MASK); } - void pruneEndInterface(InterfaceFlags end_flags) { pruneInterface(end_flags & END_IF_MASK); } + /** Prune interface to comply with the given propagation direction + * + * PropagatingEitherWay uses this in restrictDirection() */ + virtual void pruneInterface(InterfaceFlags /* accepted */) {} /// validate connectivity of children (after init() was done) virtual void validateConnectivity() const; @@ -212,10 +211,10 @@ 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; diff --git a/core/src/container.cpp b/core/src/container.cpp index a21e83b5..4bad4efd 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -425,51 +425,39 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) // called by parent asking for pruning of this' interface void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { + // we need to have some children to do the actual work if (children().empty()) - throw InitStageException(*me(), "container is empty"); + throw InitStageException(*me(), "no children"); - // TODO(v4hn): if ever there is a use case to start pruning - // with a specified end interface, this would need to be extended - if (!(accepted & (READS_START | WRITES_PREV_END))) - return; // The start interface direction is not decided + if (!(accepted & START_IF_MASK)) + throw InitStageException(*me(), "unknown start interface"); Stage& first = *children().front(); Stage& last = *children().back(); - // sweep through children once: infer and connect interfaces - first.pimpl()->pruneStartInterface(accepted); + // FIRST child + first.pimpl()->pruneInterface(accepted & START_IF_MASK); + // connect first child's (start) push interface setChildsPushBackwardInterface(first); - - for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) { - StagePrivate* child_impl = (**it).pimpl(); - StagePrivate* previous_impl = (**previous_it).pimpl(); - child_impl->pruneStartInterface(invert(previous_impl->requiredInterface())); - connect(*previous_impl, *child_impl); - } - - // potentially connect outmost push interface to pending_ buffer - setChildsPushForwardInterface(last); - - if ((accepted & END_IF_MASK) != UNKNOWN && - (last.pimpl()->requiredInterface() & END_IF_MASK) != (accepted & END_IF_MASK)) { - boost::format desc( - "requested end interface for container (%1%) does not agree with inferred end interface of last child (%2%)"); - desc % flowSymbol(accepted) % flowSymbol(last.pimpl()->requiredInterface()); - throw InitStageException(*me(), desc.str()); - } - - // if first/last pull, this needs to pull to and forward to the children + // 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); })); - else - starts_.reset(); + // process all children and connect them + for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) { + StagePrivate* child_impl = (**it).pimpl(); + StagePrivate* previous_impl = (**previous_it).pimpl(); + child_impl->pruneInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK); + connect(*previous_impl, *child_impl); + } + + // connect last child's (end) push interface + setChildsPushForwardInterface(last); + // 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); })); - else - ends_.reset(); required_interface_ = first.pimpl()->interfaceFlags() & START_IF_MASK | last.pimpl()->interfaceFlags() & END_IF_MASK; } @@ -582,11 +570,9 @@ ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase : ContainerBasePrivate(me, name) {} void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) { + // we need to have some children to do the actual work if (children().empty()) - throw InitStageException(*me(), "trying to prune empty container"); - - if (accepted == UNKNOWN) - return; // nothing to prune + throw InitStageException(*me(), "no children"); InitStageException exceptions; InterfaceFlags interface; @@ -789,24 +775,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 does not match the common one."); - } - - switch (required) { +void MergerPrivate::pruneInterface(InterfaceFlags accepted) { + ContainerBasePrivate::pruneInterface(accepted); + 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: @@ -814,7 +790,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)) {} diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 3a65f9a2..0d001c85 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -112,28 +112,6 @@ InterfaceFlags StagePrivate::interfaceFlags() const { return f; } -void StagePrivate::pruneInterface(InterfaceFlags accepted) { - const InterfaceFlags accepted_start{ accepted & START_IF_MASK }; - const InterfaceFlags accepted_end{ accepted & END_IF_MASK }; - - const InterfaceFlags required{ requiredInterface() }; - const InterfaceFlags required_start{ required & START_IF_MASK }; - const InterfaceFlags required_end{ required & END_IF_MASK }; - - if (required == UNKNOWN) - throw std::logic_error("stage type does not specify interface, but also does not override pruneInterface. " - "Who sets up its interfaces?"); - - // only one side needs to be specified but the value has to be compatible with this stage - if (((accepted_start != UNKNOWN) && (accepted_start & required_start) != required_start) || - ((accepted_end != UNKNOWN) && (accepted_end & required_end) != required_end)) { - boost::format desc("interface %1% %2% does not match inferred interface %3% %4%"); - desc % flowSymbol(required_start) % flowSymbol(required_end); - desc % flowSymbol(accepted_start) % flowSymbol(accepted_end); - throw InitStageException(*me(), desc.str()); - } -} - void StagePrivate::validateConnectivity() const { // check that the required interface is provided InterfaceFlags required = requiredInterface(); @@ -416,47 +394,53 @@ ComputeBase::ComputeBase(ComputeBasePrivate* impl) : Stage(impl) {} PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir, const std::string& name) - : ComputeBasePrivate(me, name), required_interface_dirs_(dir) {} + : ComputeBasePrivate(me, name), configured_dir_(dir) { + initInterface(dir); +} // initialize pull interfaces to match requested propagation directions void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) { - if (required_interface_dirs_ != PropagatingEitherWay::AUTO && dir != required_interface_dirs_) { - auto flow = [](PropagatingEitherWay::Direction dir) -> const char* { - switch (dir) { - case PropagatingEitherWay::AUTO: - return flowSymbol(InterfaceFlags{ READS_START, WRITES_PREV_END }); - case PropagatingEitherWay::FORWARD: - return flowSymbol(InterfaceFlags{ READS_START }); - case PropagatingEitherWay::BACKWARD: - return flowSymbol(InterfaceFlags{ WRITES_PREV_END }); - } - }; - - boost::format desc("propagation direction %1% incompatible with inferred direction %2%"); - desc % flow(required_interface_dirs_) % flow(dir); - throw InitStageException(*me(), desc.str()); + 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; } - - if (dir == PropagatingEitherWay::FORWARD) - starts_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedStarts(it); })); - else if (dir == PropagatingEitherWay::BACKWARD) - ends_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedEnds(it); })); - - required_interface_dirs_ = dir; } void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) { if (accepted == UNKNOWN) - throw InitStageException(*me(), "cannot prune to unknown interface"); + throw InitStageException(*me(), "cannot initialize to unknown interface"); + + auto dir = PropagatingEitherWay::AUTO; if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START) - initInterface(PropagatingEitherWay::FORWARD); + dir = PropagatingEitherWay::FORWARD; else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END) - initInterface(PropagatingEitherWay::BACKWARD); + dir = PropagatingEitherWay::BACKWARD; else { - boost::format desc("propagator cannot act with interfaces start(%1%), end(%2%)"); + boost::format desc("propagator cannot handle external interface %1% %2%"); desc % flowSymbol(accepted) % flowSymbol(accepted); throw InitStageException(*me(), desc.str()); } + if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) { + boost::format desc("configured interface (%1% %2%) does not match external one (%3% %4%)"); + desc % flowSymbol(required_interface_) % flowSymbol(required_interface_); + desc % flowSymbol(accepted) % flowSymbol(accepted); + throw InitStageException(*me(), desc.str()); + } + initInterface(dir); } void PropagatingEitherWayPrivate::validateConnectivity() const { @@ -475,14 +459,7 @@ void PropagatingEitherWayPrivate::validateConnectivity() const { } InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const { - switch (required_interface_dirs_) { - case PropagatingEitherWay::FORWARD: - return PROPAGATE_FORWARDS; - case PropagatingEitherWay::BACKWARD: - return PROPAGATE_BACKWARDS; - case PropagatingEitherWay::AUTO: - return UNKNOWN; - } + return required_interface_; } void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) { @@ -540,9 +517,11 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) : void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) { auto impl = pimpl(); - if (impl->required_interface_dirs_ != AUTO) + if (impl->configured_dir_ == dir) + return; + if (impl->configured_dir_ != AUTO) throw std::runtime_error("Cannot change direction after being connected"); - + impl->configured_dir_ = dir; impl->initInterface(dir); } From e56c772ecb731f3d5358e4389744f3c78e86eb46 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 10 Apr 2020 01:20:50 +0200 Subject: [PATCH 10/13] Combine pruneInterface() + validateConnectivity() Validate interfaces during resolution. No need to separately validate interfaces. Thus, validateConnectivity() is removed from Task::init(). Functions are kept (but simplified) for unit testing. --- .../moveit/task_constructor/container_p.h | 10 +- .../include/moveit/task_constructor/stage_p.h | 2 - core/src/container.cpp | 151 +++++++----------- core/src/stage.cpp | 15 -- core/src/task.cpp | 2 - 5 files changed, 68 insertions(+), 112 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index af204e00..26fb099e 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -132,9 +132,6 @@ protected: bool allowed = (required & WRITES_NEXT_START); child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } - // report error about mismatching interface (start or end as determined by mask) - template - void mismatchingInterface(InitStageException& errors, const StagePrivate& child) 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); @@ -184,6 +181,10 @@ public: protected: // connect two neighbors void connect(StagePrivate& stage1, StagePrivate& stage2); + + // validate that child's interface matches mine (considering start or end only as determined by mask) + template + void validateInterface(const StagePrivate& child, InterfaceFlags required) const; }; PIMPL_FUNCTIONS(SerialContainer) @@ -220,6 +221,9 @@ public: 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); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 572bc2d0..8e917524 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -222,8 +222,6 @@ public: 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; bool canCompute() const override; void compute() override; diff --git a/core/src/container.cpp b/core/src/container.cpp index 4bad4efd..8e8d47b2 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -91,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; -} - -template -void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child) const { - boost::format desc("%1% interface of '%2%' (%3%) does not match mine (%4%)"); - errors.push_back(*me(), (desc % (mask == START_IF_MASK ? "start" : "end") % child.name() % - flowSymbol(child.interfaceFlags()) % flowSymbol(interfaceFlags())) - .str()); + for (const auto& child : children()) + child->pimpl()->validateConnectivity(); } bool ContainerBasePrivate::canCompute() const { @@ -416,13 +400,27 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END)) stage2.setPrevEnds(stage1.ends()); else { - boost::format desc("end interface of '%1%' (%2%) does not match start interface of '%3%' (%4%)"); + boost::format desc("cannot connect end interface of '%1%' (%2%) to start interface of '%3%' (%4%)"); desc % stage1.name() % flowSymbol(flags1); desc % stage2.name() % flowSymbol(flags2); throw InitStageException(*me(), desc.str()); } } +template +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(child_interface) % flowSymbol(required); + throw InitStageException(*me_, desc.str()); + } +} + // called by parent asking for pruning of this' interface void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { // we need to have some children to do the actual work @@ -439,6 +437,8 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { first.pimpl()->pruneInterface(accepted & START_IF_MASK); // connect first child's (start) push interface setChildsPushBackwardInterface(first); + // validate that first child's and this container's start interfaces match + validateInterface(*first.pimpl(), accepted); // connect first child's (start) pull interface if (const InterfacePtr& target = first.pimpl()->starts()) starts_.reset(new Interface( @@ -454,6 +454,8 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { // connect last child's (end) push interface setChildsPushForwardInterface(last); + // validate that last child's and this container's end interfaces match + validateInterface(*last.pimpl(), accepted); // connect last child's (end) pull interface if (const InterfacePtr& target = last.pimpl()->ends()) ends_.reset(new Interface( @@ -463,30 +465,14 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { } 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); + validateInterface(*children().front()->pimpl(), mine); + validateInterface(*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); - } - - // 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. @@ -503,16 +489,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 { @@ -575,80 +558,68 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) { throw InitStageException(*me(), "no children"); InitStageException exceptions; - InterfaceFlags interface; + bool first = true; for (const Stage::pointer& child : children()) { try { - child->pimpl()->pruneInterface(accepted); + auto child_impl = child->pimpl(); + child_impl->pruneInterface(accepted); + validateInterfaces(*child_impl, accepted, first); + // initialize push connections of children according to their demands + setChildsPushForwardInterface(*child); + setChildsPushBackwardInterface(*child); + first = false; } catch (InitStageException& e) { exceptions.append(e); continue; } - - InterfaceFlags child_interface = child->pimpl()->requiredInterface(); - if (interface == UNKNOWN) - interface = child_interface; - else if ((interface & child_interface) != child_interface) { - boost::format desc("inferred interface of stage '%1%' (%2%/%3%) does not agree with the inferred interface of " - "its siblings (%4%/%5%)."); - desc % child->name(); - desc % flowSymbol(child_interface) % flowSymbol(child_interface); - desc % flowSymbol(interface) % flowSymbol(interface); - exceptions.push_back(*me(), desc.str()); - } - } - - if ((interface & accepted) != accepted) { - boost::format desc("required interface (%1%/%2%) does not match children (%3%/%4%)."); - desc % flowSymbol(accepted) % flowSymbol(accepted); - desc % flowSymbol(interface) % flowSymbol(interface); - exceptions.push_back(*me(), desc.str()); } if (exceptions) throw exceptions; // States received by the container need to be copied to all children's pull interfaces. - if (interface & READS_START) + if (accepted & READS_START) starts().reset(new Interface([this](Interface::iterator external, bool updated) { this->onNewExternalState(Interface::FORWARD, external, updated); })); - if (interface & READS_END) + if (accepted & READS_END) ends().reset(new Interface([this](Interface::iterator external, bool updated) { this->onNewExternalState(Interface::BACKWARD, external, updated); })); - // initialize push connections of children according to their demands - for (const Stage::pointer& stage : children()) { - setChildsPushForwardInterface(*stage); - setChildsPushBackwardInterface(*stage); + required_interface_ = 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)); } - required_interface_ = interface; + 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(child_interface) % flowSymbol(child_interface); + desc % flowSymbol(external) % flowSymbol(external); + throw InitStageException(*me_, desc.str()); + } } void ParallelContainerBasePrivate::validateConnectivity() const { - InitStageException errors; InterfaceFlags my_interface = interfaceFlags(); // check that input / output interfaces of all children are handled by my interface - for (const auto& child : children()) { - InterfaceFlags current = child->pimpl()->interfaceFlags(); - if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK)) - mismatchingInterface(errors, *child->pimpl()); - if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK)) - mismatchingInterface(errors, *child->pimpl()); - } + for (const auto& child : children()) + validateInterfaces(*child->pimpl(), my_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, diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 0d001c85..4ec15b17 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -443,21 +443,6 @@ void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) { initInterface(dir); } -void PropagatingEitherWayPrivate::validateConnectivity() const { - 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 (errors) - throw errors; -} - InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const { return required_interface_; } diff --git a/core/src/task.cpp b/core/src/task.cpp index 4b740968..9952c906 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -262,8 +262,6 @@ void Task::init() { 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(); // provide introspection instance to all stages impl->setIntrospection(impl->introspection_.get()); From aa732d8c666a887feb81a5f562f1a033e8ecb3a3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 10 Apr 2020 10:45:14 +0200 Subject: [PATCH 11/13] simplify internal API --- .../moveit/task_constructor/container_p.h | 16 +++++++--------- core/src/container.cpp | 8 ++++---- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 26fb099e..226e7efd 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -119,18 +119,16 @@ 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(); + // 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.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr()); + child->setPrevEnds(allowed ? pending_backward_ : InterfacePtr()); } - inline void setChildsPushForwardInterface(Stage& child) { - InterfaceFlags required = child.pimpl()->requiredInterface(); + inline void setChildsPushForwardInterface(StagePrivate* child) { + InterfaceFlags required = child->requiredInterface(); bool allowed = (required & WRITES_NEXT_START); - child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); + child->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } /// copy external_state to a child's interface and remember the link in internal_to map diff --git a/core/src/container.cpp b/core/src/container.cpp index 8e8d47b2..e90906d3 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -436,7 +436,7 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { // FIRST child first.pimpl()->pruneInterface(accepted & START_IF_MASK); // connect first child's (start) push interface - setChildsPushBackwardInterface(first); + setChildsPushBackwardInterface(first.pimpl()); // validate that first child's and this container's start interfaces match validateInterface(*first.pimpl(), accepted); // connect first child's (start) pull interface @@ -453,7 +453,7 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { } // connect last child's (end) push interface - setChildsPushForwardInterface(last); + setChildsPushForwardInterface(last.pimpl()); // validate that last child's and this container's end interfaces match validateInterface(*last.pimpl(), accepted); // connect last child's (end) pull interface @@ -566,8 +566,8 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) { child_impl->pruneInterface(accepted); validateInterfaces(*child_impl, accepted, first); // initialize push connections of children according to their demands - setChildsPushForwardInterface(*child); - setChildsPushBackwardInterface(*child); + setChildsPushForwardInterface(child_impl); + setChildsPushBackwardInterface(child_impl); first = false; } catch (InitStageException& e) { exceptions.append(e); From 499fcfb04b9a359ceaf2f26c5b391b24aea7e679 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 10 Apr 2020 19:53:25 +0200 Subject: [PATCH 12/13] cleanup / renaming * Rename pruneInterface() -> resolveInterface() * Rename accepted (interface) -> expected * Improve exception strings --- .../moveit/task_constructor/container_p.h | 10 +-- .../include/moveit/task_constructor/stage_p.h | 14 ++-- core/src/container.cpp | 28 ++++---- core/src/stage.cpp | 18 ++--- core/src/task.cpp | 2 +- core/test/test_container.cpp | 18 ++--- demo/scripts/grasp.py | 46 +++++++++++++ demo/scripts/pick.py | 65 +++++++++++++++++++ notes.org | 53 +++++++++++++++ pick+place.org | 28 ++++++++ test.py | 16 +++++ 11 files changed, 251 insertions(+), 47 deletions(-) create mode 100644 demo/scripts/grasp.py create mode 100644 demo/scripts/pick.py create mode 100644 notes.org create mode 100644 pick+place.org create mode 100644 test.py diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 226e7efd..6982778d 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -106,7 +106,7 @@ public: void validateConnectivity() const override; // Containers derive their required interface from their children - // UNKNOWN until pruneInterface was called + // UNKNOWN until resolveInterface was called InterfaceFlags requiredInterface() const override { return required_interface_; } // forward these methods to the public interface for containers @@ -139,7 +139,7 @@ protected: auto& internalToExternalMap() { return internal_to_external_; } const auto& internalToExternalMap() const { return internal_to_external_; } - // set in pruneInterface() + // set in resolveInterface() InterfaceFlags required_interface_; private: @@ -170,7 +170,7 @@ public: SerialContainerPrivate(SerialContainer* me, const std::string& name); // 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; @@ -215,7 +215,7 @@ public: ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name); // called by parent asking for pruning of this' interface - void pruneInterface(InterfaceFlags accepted) override; + void resolveInterface(InterfaceFlags expected) override; void validateConnectivity() const override; @@ -251,7 +251,7 @@ public: typedef std::function Spawner; MergerPrivate(Merger* me, const std::string& name); - void pruneInterface(InterfaceFlags accepted) override; + void resolveInterface(InterfaceFlags expected) override; void onNewPropagateSolution(const SolutionBase& s); void onNewGeneratorSolution(const SolutionBase& s); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 8e917524..a8675241 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -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 UNKNOWN */ + /// 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; @@ -220,8 +216,8 @@ public: 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; + // resolve interface to the given propagation direction + void resolveInterface(InterfaceFlags expected) override; bool canCompute() const override; void compute() override; diff --git a/core/src/container.cpp b/core/src/container.cpp index e90906d3..df66b263 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -422,23 +422,23 @@ void SerialContainerPrivate::validateInterface(const StagePrivate& child, Interf } // 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()) throw InitStageException(*me(), "no children"); - if (!(accepted & START_IF_MASK)) + if (!(expected & START_IF_MASK)) throw InitStageException(*me(), "unknown start interface"); Stage& first = *children().front(); Stage& last = *children().back(); // FIRST child - first.pimpl()->pruneInterface(accepted & START_IF_MASK); + 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(*first.pimpl(), accepted); + validateInterface(*first.pimpl(), expected); // connect first child's (start) pull interface if (const InterfacePtr& target = first.pimpl()->starts()) starts_.reset(new Interface( @@ -448,14 +448,14 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) { StagePrivate* child_impl = (**it).pimpl(); StagePrivate* previous_impl = (**previous_it).pimpl(); - child_impl->pruneInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK); + child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK); connect(*previous_impl, *child_impl); } // connect last child's (end) push interface setChildsPushForwardInterface(last.pimpl()); // validate that last child's and this container's end interfaces match - validateInterface(*last.pimpl(), accepted); + validateInterface(*last.pimpl(), expected); // connect last child's (end) pull interface if (const InterfacePtr& target = last.pimpl()->ends()) ends_.reset(new Interface( @@ -552,7 +552,7 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name) : ContainerBasePrivate(me, name) {} -void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) { +void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) { // we need to have some children to do the actual work if (children().empty()) throw InitStageException(*me(), "no children"); @@ -563,8 +563,8 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) { for (const Stage::pointer& child : children()) { try { auto child_impl = child->pimpl(); - child_impl->pruneInterface(accepted); - validateInterfaces(*child_impl, accepted, first); + child_impl->resolveInterface(expected); + validateInterfaces(*child_impl, expected, first); // initialize push connections of children according to their demands setChildsPushForwardInterface(child_impl); setChildsPushBackwardInterface(child_impl); @@ -579,16 +579,16 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) { throw exceptions; // States received by the container need to be copied to all children's pull interfaces. - if (accepted & READS_START) + if (expected & READS_START) starts().reset(new Interface([this](Interface::iterator external, bool updated) { this->onNewExternalState(Interface::FORWARD, external, updated); })); - if (accepted & READS_END) + if (expected & READS_END) ends().reset(new Interface([this](Interface::iterator external, bool updated) { this->onNewExternalState(Interface::BACKWARD, external, updated); })); - required_interface_ = accepted; + required_interface_ = expected; } void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external, @@ -746,8 +746,8 @@ void Fallbacks::onNewSolution(const SolutionBase& s) { MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} -void MergerPrivate::pruneInterface(InterfaceFlags accepted) { - ContainerBasePrivate::pruneInterface(accepted); +void MergerPrivate::resolveInterface(InterfaceFlags expected) { + ContainerBasePrivate::resolveInterface(expected); switch (interfaceFlags()) { case PROPAGATE_FORWARDS: case PROPAGATE_BACKWARDS: diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 4ec15b17..c3e2665c 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -117,7 +117,7 @@ void StagePrivate::validateConnectivity() const { InterfaceFlags required = requiredInterface(); InterfaceFlags actual = interfaceFlags(); if ((required & actual) != required) { - boost::format desc("interface %1% %2% does not satisfy required interface %3% %4%"); + boost::format desc("actual interface %1% %2% does not match required interface %3% %4%"); desc % flowSymbol(actual) % flowSymbol(actual); desc % flowSymbol(required) % flowSymbol(required); throw InitStageException(*me(), desc.str()); @@ -420,24 +420,24 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction } } -void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) { - if (accepted == UNKNOWN) +void PropagatingEitherWayPrivate::resolveInterface(InterfaceFlags expected) { + if (expected == UNKNOWN) throw InitStageException(*me(), "cannot initialize to unknown interface"); auto dir = PropagatingEitherWay::AUTO; - if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START) + if ((expected & START_IF_MASK) == READS_START || (expected & END_IF_MASK) == WRITES_NEXT_START) dir = PropagatingEitherWay::FORWARD; - else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END) + else if ((expected & START_IF_MASK) == WRITES_PREV_END || (expected & END_IF_MASK) == READS_END) dir = PropagatingEitherWay::BACKWARD; else { - boost::format desc("propagator cannot handle external interface %1% %2%"); - desc % flowSymbol(accepted) % flowSymbol(accepted); + boost::format desc("propagator cannot satisfy expected interface %1% %2%"); + desc % flowSymbol(expected) % flowSymbol(expected); throw InitStageException(*me(), desc.str()); } if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) { - boost::format desc("configured interface (%1% %2%) does not match external one (%3% %4%)"); + boost::format desc("configured interface (%1% %2%) does not match expected one (%3% %4%)"); desc % flowSymbol(required_interface_) % flowSymbol(required_interface_); - desc % flowSymbol(accepted) % flowSymbol(accepted); + desc % flowSymbol(expected) % flowSymbol(expected); throw InitStageException(*me(), desc.str()); } initInterface(dir); diff --git a/core/src/task.cpp b/core/src/task.cpp index 9952c906..29e1b8e3 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -261,7 +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 })); + stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE })); // provide introspection instance to all stages impl->setIntrospection(impl->introspection_.get()); diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index b6853104..b5f7d2ae 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -200,19 +200,19 @@ protected: append(container, types); try { container.init(robot_model); - // prune 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 - accepted |= READS_START; + expected |= READS_START; if (end) - accepted |= WRITES_NEXT_START; + expected |= WRITES_NEXT_START; else - accepted |= READS_END; + expected |= READS_END; - container.pimpl()->pruneInterface(accepted); + container.pimpl()->resolveInterface(expected); container.pimpl()->validateConnectivity(); if (!expect_failure) return; // as expected @@ -456,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); // -> -> diff --git a/demo/scripts/grasp.py b/demo/scripts/grasp.py new file mode 100644 index 00000000..cf313acc --- /dev/null +++ b/demo/scripts/grasp.py @@ -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() diff --git a/demo/scripts/pick.py b/demo/scripts/pick.py new file mode 100644 index 00000000..f83a4f5e --- /dev/null +++ b/demo/scripts/pick.py @@ -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() diff --git a/notes.org b/notes.org new file mode 100644 index 00000000..eb6bac4c --- /dev/null +++ b/notes.org @@ -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 diff --git a/pick+place.org b/pick+place.org new file mode 100644 index 00000000..f30f8d6b --- /dev/null +++ b/pick+place.org @@ -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 + diff --git a/test.py b/test.py new file mode 100644 index 00000000..368ddcd8 --- /dev/null +++ b/test.py @@ -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" From c4d0ab06362792779cc4b0403dfc3de576878ff0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 10 Apr 2020 20:25:54 +0200 Subject: [PATCH 13/13] SerialContainer: Resolve interfaces of all stages --- core/src/container.cpp | 59 ++++++++++++++++++++++++++---------------- 1 file changed, 37 insertions(+), 22 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index df66b263..438976a3 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -433,35 +433,50 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { Stage& first = *children().front(); Stage& last = *children().back(); - // 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(*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); })); + 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(*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); + } // process all children and connect them for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) { - 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); + 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); + } } - // connect last child's (end) push interface - setChildsPushForwardInterface(last.pimpl()); - // validate that last child's and this container's end interfaces match - validateInterface(*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); })); + try { // connect last child's (end) push interface + setChildsPushForwardInterface(last.pimpl()); + // validate that last child's and this container's end interfaces match + validateInterface(*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); + } required_interface_ = first.pimpl()->interfaceFlags() & START_IF_MASK | last.pimpl()->interfaceFlags() & END_IF_MASK; + + if (exceptions) + throw exceptions; } void SerialContainerPrivate::validateConnectivity() const {