mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
enforce one-parent policy in StagePrivate
So new containers cannot get this wrong by accident.
This commit is contained in:
parent
90f5d22e83
commit
df8a783239
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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 {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user