From d9a38f46ab1c82d859d3c2e0f465e72eccac5956 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 2 May 2019 23:26:32 +0200 Subject: [PATCH] SerialContainer: fix nested interface resolution We need to distinguish two cases for how the interface of a nested serial container is determined: 1. from its children 2. from its (outer) context As long as the interface is not fully resolved, requiredInterface() returns UNKNOWN. After pruning, the first/last child's interface is remembered and reported instead. --- .../moveit/task_constructor/container_p.h | 6 ++++ core/src/container.cpp | 31 +++++++++++++++---- 2 files changed, 31 insertions(+), 6 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index fe05a772..436ec1cb 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -178,6 +178,12 @@ private: 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_; }; PIMPL_FUNCTIONS(SerialContainer) diff --git a/core/src/container.cpp b/core/src/container.cpp index 0e3924b0..fd21b66c 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -359,14 +359,17 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s : ContainerBasePrivate(me, name) {} -// a serial container's required interface is derived from the actual input interface -// of the first child and the actual output interface of the last child +// 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_; + if (children().empty()) return UNKNOWN; - return (children().front()->pimpl()->interfaceFlags() & START_IF_MASK) - | (children().back()->pimpl()->interfaceFlags() & END_IF_MASK); + 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 @@ -415,6 +418,7 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model) 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); @@ -455,6 +459,16 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model) 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 void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (children().empty()) return; @@ -483,7 +497,7 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (!children().back()->pimpl()->ends()) ends_.reset(); - if (interfaceFlags() == InterfaceFlags()) + if (interfaceFlags() == UNKNOWN) throw InitStageException(*me(), "failed to derive propagation direction"); } @@ -492,7 +506,10 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end) { - if (first == end) return; // nothing to do in this case + if (first == end) { + storeRequiredInterface(first, end); + return; // nothing to do in this case + } // determine accepted interface from available push interfaces InterfaceFlags accepted; @@ -549,6 +566,8 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs StagePrivate* impl = (*it)->pimpl(); impl->pruneInterface(accepted); } + + storeRequiredInterface(first, end); } void SerialContainerPrivate::validateConnectivity() const