enforce one-parent policy in StagePrivate

So new containers cannot get this wrong by accident.
This commit is contained in:
v4hn 2020-03-20 16:36:10 +01:00
parent 90f5d22e83
commit df8a783239
3 changed files with 37 additions and 15 deletions

View File

@ -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 @@ 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

View File

@ -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;
}

View File

@ -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 {