From df8a783239025f5df0e498d67f103d66bf8cd3ce Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 20 Mar 2020 16:36:10 +0100 Subject: [PATCH] enforce one-parent policy in StagePrivate So new containers cannot get this wrong by accident. --- .../include/moveit/task_constructor/stage_p.h | 29 +++++++++++++++---- core/src/container.cpp | 9 ++---- core/src/task.cpp | 14 ++++++--- 3 files changed, 37 insertions(+), 15 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 94de2e9a..84d0fc31 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -41,6 +41,9 @@ #include #include #include + +#include + #include #include @@ -112,15 +115,31 @@ public: return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); } - /// the following methods should be called only by a container - /// to setup the connection structure of their children - inline void setHierarchy(ContainerBase* parent, container_type::iterator it) { + /// set parent of stage + /// enforce only one parent exists + inline bool setParent(ContainerBase* parent) { + if (parent_) { + ROS_ERROR_STREAM("Tried to add stage '" << name() << "' to two parents"); + return false; // it's not allowed to add a stage to a parent if it already has one + } parent_ = parent; - it_ = it; + return true; } + + /// explicitly orphan stage + inline void unparent() { + parent_ = nullptr; + it_ = container_type::iterator(); + } + + /// the following methods should be called only by the current parent + /// to setup the connection structure of their children + inline void setParentPosition(container_type::iterator it) { it_ = it; } + inline void setIntrospection(Introspection* introspection) { introspection_ = introspection; } + inline void setPrevEnds(const InterfacePtr& prev_ends) { prev_ends_ = prev_ends; } inline void setNextStarts(const InterfacePtr& next_starts) { next_starts_ = next_starts; } - inline void setIntrospection(Introspection* introspection) { introspection_ = introspection; } + void composePropertyErrorMsg(const std::string& name, std::ostream& os); // methods to spawn new solutions diff --git a/core/src/container.cpp b/core/src/container.cpp index 7e0d5c33..4420c42e 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -196,14 +196,11 @@ bool ContainerBase::traverseRecursively(const ContainerBase::StageCallback& proc bool ContainerBase::insert(Stage::pointer&& stage, int before) { StagePrivate* impl = stage->pimpl(); - if (impl->parent() != nullptr || !stage->solutions().empty() || !stage->failures().empty()) { - ROS_ERROR("cannot re-parent stage"); + if (!impl->setParent(this)) return false; - } - ContainerBasePrivate::const_iterator where = pimpl()->childByIndex(before, true); ContainerBasePrivate::iterator it = pimpl()->children_.insert(where, std::move(stage)); - impl->setHierarchy(this, it); + impl->setParentPosition(it); return true; } @@ -211,7 +208,7 @@ bool ContainerBasePrivate::remove(ContainerBasePrivate::const_iterator pos) { if (pos == children_.end()) return false; - (*pos)->pimpl()->setHierarchy(nullptr, ContainerBasePrivate::iterator()); + (*pos)->pimpl()->unparent(); children_.erase(pos); return true; } diff --git a/core/src/task.cpp b/core/src/task.cpp index 3d590303..5097adfc 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -86,12 +86,18 @@ void swap(StagePrivate*& lhs, StagePrivate*& rhs) { // and redirect the parent pointers of children to new parents auto& lhs_children = static_cast(lhs)->children_; - for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it) - (*it)->pimpl()->setHierarchy(static_cast(lhs->me_), it); + for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it) { + (*it)->pimpl()->unparent(); + (*it)->pimpl()->setParent(static_cast(lhs->me_)); + (*it)->pimpl()->setParentPosition(it); + } auto& rhs_children = static_cast(rhs)->children_; - for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it) - (*it)->pimpl()->setHierarchy(static_cast(rhs->me_), it); + for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it) { + (*it)->pimpl()->unparent(); + (*it)->pimpl()->setParent(static_cast(rhs->me_)); + (*it)->pimpl()->setParentPosition(it); + } } const ContainerBase* TaskPrivate::stages() const {