diff --git a/include/moveit_task_constructor/container.h b/include/moveit_task_constructor/container.h index ffdea3fd..4cf36af2 100644 --- a/include/moveit_task_constructor/container.h +++ b/include/moveit_task_constructor/container.h @@ -9,7 +9,7 @@ class ContainerBase : public SubTask { public: PRIVATE_CLASS(ContainerBase) - typedef std::unique_ptr value_type; + typedef SubTask::pointer value_type; typedef std::function StageCallback; typedef std::function&)> SolutionCallback; diff --git a/include/moveit_task_constructor/subtask.h b/include/moveit_task_constructor/subtask.h index bd6d018a..392eea10 100644 --- a/include/moveit_task_constructor/subtask.h +++ b/include/moveit_task_constructor/subtask.h @@ -34,17 +34,16 @@ class InterfaceState; typedef std::pair InterfaceStatePair; -class ContainerBasePrivate; class SubTaskPrivate; class SubTask { friend std::ostream& operator<<(std::ostream &os, const SubTask& stage); friend class SubTaskPrivate; - friend class ContainerBasePrivate; public: inline const SubTaskPrivate* pimpl_func() const { return pimpl_; } inline SubTaskPrivate* pimpl_func() { return pimpl_; } + typedef std::unique_ptr pointer; enum InterfaceFlag { READS_INPUT = 0x01, READS_OUTPUT = 0x02, diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index e39be6a6..4b0c3c2a 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -28,7 +28,7 @@ public: Task(const std::string &name = std::string()); - void add(std::unique_ptr &&stage); + void add(SubTask::pointer &&stage); using SerialContainer::clear; bool plan(); diff --git a/src/container.cpp b/src/container.cpp index 50be978a..183239df 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -22,49 +22,9 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before) bool ContainerBasePrivate::canInsert(const SubTask &stage) const { const SubTaskPrivate* impl = stage.pimpl_func(); return impl->parent_ == nullptr // re-parenting is not supported - && impl->predeccessor_ == nullptr - && impl->successor_ == nullptr && impl->trajectories_.empty(); // existing trajectories would become invalid } -// insert child into chain, before where -void ContainerBasePrivate::insertSerial(ContainerBasePrivate::value_type &&child, ContainerBasePrivate::const_iterator before) { - SubTaskPrivate* child_impl = child->pimpl_func(); - bool at_end = before == children_.cend(); - bool at_begin = before == children_.cbegin(); - - // child should not be connected yet - assert(child_impl->parent_ == nullptr); - assert(child_impl->predeccessor_ == nullptr); - assert(child_impl->successor_ == nullptr); - - child_impl->parent_ = this; - if (children_.empty()) { - child_impl->successor_ = this; - child_impl->predeccessor_ = this; - } else if (at_end) { - const_iterator prev = before; --prev; - SubTaskPrivate* prev_impl = prev->get()->pimpl_func(); - child_impl->successor_ = this; - child_impl->predeccessor_ = prev_impl; - prev_impl->successor_ = child_impl; - } else if (at_begin) { - SubTaskPrivate* next_impl = before->get()->pimpl_func(); - child_impl->predeccessor_ = this; - child_impl->successor_ = next_impl; - next_impl->predeccessor_ = child_impl; - } else { - const_iterator prev = before; --prev; - SubTaskPrivate* prev_impl = prev->get()->pimpl_func(); - SubTaskPrivate* next_impl = before->get()->pimpl_func(); - child_impl->successor_ = next_impl; - child_impl->predeccessor_ = prev_impl; - next_impl->predeccessor_ = child_impl; - prev_impl->successor_ = child_impl; - } - ContainerBasePrivate::insert(std::move(child), before); -} - bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &processor, int depth) const { for (auto &stage : children_) { if (!processor(*stage, depth)) @@ -77,9 +37,12 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr } ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&subtask, ContainerBasePrivate::const_iterator pos) { + SubTaskPrivate *impl = subtask->pimpl_func(); + impl->parent_ = this; subtask->setPlanningScene(scene_); subtask->setPlanningPipeline(planner_); - return children_.insert(pos, std::move(subtask)); + impl->it_ = children_.insert(pos, std::move(subtask)); + return impl->it_; } @@ -106,6 +69,22 @@ bool SerialContainerPrivate::canInsert(const ContainerBasePrivate::value_type &s return ContainerBasePrivate::canInsert(*subtask); } +const SubTaskPrivate *SerialContainerPrivate::prev_(const SubTaskPrivate *child) const +{ + assert(parent(child) == this); + if (it(child) == children().begin()) return this; + iterator prev = it(child); --prev; + return (*prev)->pimpl_func(); +} + +const SubTaskPrivate *SerialContainerPrivate::next_(const SubTaskPrivate *child) const +{ + assert(parent(child) == this); + if (it(child) == --children().end()) return this; + iterator next = it(child); ++next; + return (*next)->pimpl_func(); +} + PRIVATE_CLASS_IMPL(SerialContainer) SerialContainer::SerialContainer(SerialContainerPrivate *impl) @@ -129,21 +108,21 @@ bool SerialContainer::insert(value_type&& subtask, int before) if (!impl->canInsert(subtask, where)) return false; - impl->insertSerial(std::move(subtask), where); + impl->insert(std::move(subtask), where); return true; } bool SerialContainer::canCompute() const { IMPL(const SerialContainer); - return impl->children_.size() > 0; + return impl->children().size() > 0; } bool SerialContainer::compute() { IMPL(SerialContainer); bool computed = false; - for(const auto& stage : impl->children_){ + for(const auto& stage : impl->children()){ if(!stage->canCompute()) continue; std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl; diff --git a/src/container_p.h b/src/container_p.h index 3a8345c9..bb09b628 100644 --- a/src/container_p.h +++ b/src/container_p.h @@ -9,25 +9,19 @@ class ContainerBasePrivate : public SubTaskPrivate { public: typedef ContainerBase::value_type value_type; - typedef std::vector array_type; + typedef SubTaskPrivate::array_type array_type; typedef array_type::iterator iterator; typedef array_type::const_iterator const_iterator; - array_type children_; - + inline const array_type& children() const { return children_; } const_iterator position(int before = -1) const; bool canInsert(const SubTask& stage) const; + iterator insert(value_type &&subtask, const_iterator pos); + inline void clear() { children_.clear(); } - /* SerialContainer doesn't have own input_, output_ interfaces, - * but share the interface pointer with their first resp. last child stage. - * In this fashion, spawned states directly get propagated to the actual stage. - * Consequently, when the container is empty, both interface pointers are invalid. */ - void insertSerial(value_type&& child, const_iterator before); - - void clear() { - children_.clear(); - } + virtual const SubTaskPrivate* prev_(const SubTaskPrivate* child) const = 0; + virtual const SubTaskPrivate* next_(const SubTaskPrivate* child) const = 0; bool traverseStages(const ContainerBase::StageCallback &processor, int depth) const; @@ -35,9 +29,11 @@ protected: ContainerBasePrivate(ContainerBase *me, const std::string &name) : SubTaskPrivate(me, name) {} + inline const ContainerBasePrivate* parent(const SubTaskPrivate *child) const { return child->parent_; } + inline iterator it(const SubTaskPrivate *child) const { return child->it_; } private: - iterator insert(value_type &&subtask, const_iterator pos); + array_type children_; }; @@ -51,6 +47,9 @@ public: } bool canInsert(const value_type& subtask, const_iterator before) const; + + const SubTaskPrivate* prev_(const SubTaskPrivate* child) const override; + const SubTaskPrivate* next_(const SubTaskPrivate* child) const override; }; diff --git a/src/subtask.cpp b/src/subtask.cpp index 076ae9a5..1074f7b5 100644 --- a/src/subtask.cpp +++ b/src/subtask.cpp @@ -1,4 +1,5 @@ #include "subtask_p.h" +#include "container_p.h" #include #include @@ -92,6 +93,13 @@ void SubTaskPrivate::sendBackward(SubTrajectory& trajectory, const planning_scen prevOutput()->add(ps, NULL, &trajectory); } +const SubTaskPrivate* SubTaskPrivate::prev() const { + return parent_ ? parent_->prev_(this) : nullptr; +} +const SubTaskPrivate* SubTaskPrivate::next() const { + return parent_ ? parent_->next_(this) : nullptr; +} + PRIVATE_CLASS_IMPL(PropagatingAnyWay) PropagatingAnyWay::PropagatingAnyWay(const std::string &name) diff --git a/src/subtask_p.h b/src/subtask_p.h index 0b9614bb..6d0c310d 100644 --- a/src/subtask_p.h +++ b/src/subtask_p.h @@ -7,16 +7,18 @@ namespace moveit { namespace task_constructor { +class ContainerBasePrivate; class SubTaskPrivate { friend class SubTask; friend class SubTaskTest; // allow unit tests - // ContainerBase will maintain the double-linked list of interfaces - friend class ContainerBasePrivate; + friend class ContainerBasePrivate; // allow to set parent_ and it_ friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage); public: + typedef std::list array_type; + inline SubTaskPrivate(SubTask* me, const std::string& name) - : me_(me), name_(name), parent_(nullptr), predeccessor_(nullptr), successor_(nullptr) + : me_(me), name_(name), parent_(nullptr) {} SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost); @@ -36,14 +38,15 @@ public: InterfacePtr output_; std::list trajectories_; - const InterfacePtr prevOutput() const { return predeccessor_ ? predeccessor_->output_ : InterfacePtr(); } - const InterfacePtr nextInput() const { return successor_ ? successor_->input_ : InterfacePtr(); } + const SubTaskPrivate* prev() const; + const SubTaskPrivate* next() const; + const InterfacePtr prevOutput() const { const SubTaskPrivate* other = prev(); return other ? other->output_ : InterfacePtr(); } + const InterfacePtr nextInput() const { const SubTaskPrivate* other = next(); return other ? other->input_ : InterfacePtr(); } private: // items accessed by ContainerBasePrivate only to maintain hierarchy - SubTaskPrivate* parent_; - SubTaskPrivate* predeccessor_; - SubTaskPrivate* successor_; + ContainerBasePrivate* parent_; + array_type::iterator it_; }; std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage); diff --git a/src/task.cpp b/src/task.cpp index dd96bcf5..7ac3e0c7 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -74,7 +74,7 @@ Task::Task(const std::string &name) { } -void Task::add(std::unique_ptr &&stage) { +void Task::add(pointer &&stage) { if (!stage) throw std::runtime_error("Task::add() failed: invalid stage pointer"); if (!insert(std::move(stage))) diff --git a/src/test/container.cpp b/src/test/container.cpp index 1261fba4..5013a984 100644 --- a/src/test/container.cpp +++ b/src/test/container.cpp @@ -40,20 +40,20 @@ protected: void TearDown() {} // accessors for private elements of SubTaskPrivate - const SubTaskPrivate* parent(const SubTaskPrivate* p) const { return p->parent_; } - const SubTaskPrivate* prev(const SubTaskPrivate* p) const { return p->predeccessor_; } - const SubTaskPrivate* next(const SubTaskPrivate* p) const { return p->successor_; } + inline const SubTaskPrivate* parent(const SubTaskPrivate* p) const { return p->parent_; } + inline const SubTaskPrivate* prev(const SubTaskPrivate* p) const { return p->prev(); } + inline const SubTaskPrivate* next(const SubTaskPrivate* p) const { return p->next(); } void validateOrder(const SerialContainerPrivate* container, const std::initializer_list &expected) { - size_t num = container->children_.size(); + size_t num = container->children().size(); ASSERT_TRUE(num == expected.size()) << "different list lengths"; // validate position() - EXPECT_EQ(container->children_.begin(), container->position(-(num+1))); - EXPECT_EQ(container->children_.end(), container->position(num)); + EXPECT_EQ(container->children().begin(), container->position(-(num+1))); + EXPECT_EQ(container->children().end(), container->position(num)); // print order - for (auto it = container->children_.begin(), end = container->children_.end(); it != end; ++it) + for (auto it = container->children().begin(), end = container->children().end(); it != end; ++it) PRINTF(" %p", (*it)->pimpl_func()); PRINTF(" *** parent: %p ***\n", container); @@ -62,7 +62,7 @@ protected: const SubTaskPrivate* successor = container; size_t pos = 0; auto exp_it = expected.begin(); - for (auto it = container->children_.begin(), end = container->children_.end(); it != end; ++it, ++exp_it, ++pos) { + for (auto it = container->children().begin(), end = container->children().end(); it != end; ++it, ++exp_it, ++pos) { SubTaskPrivate *child = (*it)->pimpl_func(); EXPECT_EQ(child, *exp_it) << "wrong order"; EXPECT_EQ(child->parent_, container) << "wrong parent";