enforce one-parent policy in StagePrivate

So new containers cannot get this wrong by accident.

Author: v4hn

core/include/moveit/task_constructor/stage_p.h

@@ -41,6 +41,9 @@
 #include <moveit/task_constructor/stage.h>
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/cost_queue.h>
+
+#include <ros/ros.h>
+
 #include <ostream>
 #include <chrono>
 
@@ -112,15 +115,31 @@ class StagePrivate
 		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

core/src/container.cpp

@@ -196,22 +196,19 @@ 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;
 }
 
 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;
 }

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<ContainerBasePrivate*>(lhs)->children_;
-	for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it)
-		(*it)->pimpl()->setHierarchy(static_cast<ContainerBase*>(lhs->me_), it);
+	for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it) {
+		(*it)->pimpl()->unparent();
+		(*it)->pimpl()->setParent(static_cast<ContainerBase*>(lhs->me_));
+		(*it)->pimpl()->setParentPosition(it);
+	}
 
 	auto& rhs_children = static_cast<ContainerBasePrivate*>(rhs)->children_;
-	for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it)
-		(*it)->pimpl()->setHierarchy(static_cast<ContainerBase*>(rhs->me_), it);
+	for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it) {
+		(*it)->pimpl()->unparent();
+		(*it)->pimpl()->setParent(static_cast<ContainerBase*>(rhs->me_));
+		(*it)->pimpl()->setParentPosition(it);
+	}
 }
 
 const ContainerBase* TaskPrivate::stages() const {