implement children_ as std::list

This provides a double-linked list already.
Now need to manually track predeccessors and successors.
This commit is contained in:
Robert Haschke 2017-10-03 17:30:32 +02:00
parent bb06eda33c
commit 11b6dd2efd
9 changed files with 66 additions and 78 deletions

View File

@ -9,7 +9,7 @@ class ContainerBase : public SubTask
{ {
public: public:
PRIVATE_CLASS(ContainerBase) PRIVATE_CLASS(ContainerBase)
typedef std::unique_ptr<SubTask> value_type; typedef SubTask::pointer value_type;
typedef std::function<bool(const SubTask&, int depth)> StageCallback; typedef std::function<bool(const SubTask&, int depth)> StageCallback;
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback; typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;

View File

@ -34,17 +34,16 @@ class InterfaceState;
typedef std::pair<const InterfaceState&, const InterfaceState&> InterfaceStatePair; typedef std::pair<const InterfaceState&, const InterfaceState&> InterfaceStatePair;
class ContainerBasePrivate;
class SubTaskPrivate; class SubTaskPrivate;
class SubTask { class SubTask {
friend std::ostream& operator<<(std::ostream &os, const SubTask& stage); friend std::ostream& operator<<(std::ostream &os, const SubTask& stage);
friend class SubTaskPrivate; friend class SubTaskPrivate;
friend class ContainerBasePrivate;
public: public:
inline const SubTaskPrivate* pimpl_func() const { return pimpl_; } inline const SubTaskPrivate* pimpl_func() const { return pimpl_; }
inline SubTaskPrivate* pimpl_func() { return pimpl_; } inline SubTaskPrivate* pimpl_func() { return pimpl_; }
typedef std::unique_ptr<SubTask> pointer;
enum InterfaceFlag { enum InterfaceFlag {
READS_INPUT = 0x01, READS_INPUT = 0x01,
READS_OUTPUT = 0x02, READS_OUTPUT = 0x02,

View File

@ -28,7 +28,7 @@ public:
Task(const std::string &name = std::string()); Task(const std::string &name = std::string());
void add(std::unique_ptr<SubTask> &&stage); void add(SubTask::pointer &&stage);
using SerialContainer::clear; using SerialContainer::clear;
bool plan(); bool plan();

View File

@ -22,49 +22,9 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before)
bool ContainerBasePrivate::canInsert(const SubTask &stage) const { bool ContainerBasePrivate::canInsert(const SubTask &stage) const {
const SubTaskPrivate* impl = stage.pimpl_func(); const SubTaskPrivate* impl = stage.pimpl_func();
return impl->parent_ == nullptr // re-parenting is not supported return impl->parent_ == nullptr // re-parenting is not supported
&& impl->predeccessor_ == nullptr
&& impl->successor_ == nullptr
&& impl->trajectories_.empty(); // existing trajectories would become invalid && 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 { bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &processor, int depth) const {
for (auto &stage : children_) { for (auto &stage : children_) {
if (!processor(*stage, depth)) 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) { ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&subtask, ContainerBasePrivate::const_iterator pos) {
SubTaskPrivate *impl = subtask->pimpl_func();
impl->parent_ = this;
subtask->setPlanningScene(scene_); subtask->setPlanningScene(scene_);
subtask->setPlanningPipeline(planner_); 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); 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) PRIVATE_CLASS_IMPL(SerialContainer)
SerialContainer::SerialContainer(SerialContainerPrivate *impl) SerialContainer::SerialContainer(SerialContainerPrivate *impl)
@ -129,21 +108,21 @@ bool SerialContainer::insert(value_type&& subtask, int before)
if (!impl->canInsert(subtask, where)) if (!impl->canInsert(subtask, where))
return false; return false;
impl->insertSerial(std::move(subtask), where); impl->insert(std::move(subtask), where);
return true; return true;
} }
bool SerialContainer::canCompute() const bool SerialContainer::canCompute() const
{ {
IMPL(const SerialContainer); IMPL(const SerialContainer);
return impl->children_.size() > 0; return impl->children().size() > 0;
} }
bool SerialContainer::compute() bool SerialContainer::compute()
{ {
IMPL(SerialContainer); IMPL(SerialContainer);
bool computed = false; bool computed = false;
for(const auto& stage : impl->children_){ for(const auto& stage : impl->children()){
if(!stage->canCompute()) if(!stage->canCompute())
continue; continue;
std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl; std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl;

View File

@ -9,25 +9,19 @@ class ContainerBasePrivate : public SubTaskPrivate
{ {
public: public:
typedef ContainerBase::value_type value_type; typedef ContainerBase::value_type value_type;
typedef std::vector<value_type> array_type; typedef SubTaskPrivate::array_type array_type;
typedef array_type::iterator iterator; typedef array_type::iterator iterator;
typedef array_type::const_iterator const_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; const_iterator position(int before = -1) const;
bool canInsert(const SubTask& stage) 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, virtual const SubTaskPrivate* prev_(const SubTaskPrivate* child) const = 0;
* but share the interface pointer with their first resp. last child stage. virtual const SubTaskPrivate* next_(const SubTaskPrivate* child) const = 0;
* 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();
}
bool traverseStages(const ContainerBase::StageCallback &processor, int depth) const; bool traverseStages(const ContainerBase::StageCallback &processor, int depth) const;
@ -35,9 +29,11 @@ protected:
ContainerBasePrivate(ContainerBase *me, const std::string &name) ContainerBasePrivate(ContainerBase *me, const std::string &name)
: SubTaskPrivate(me, 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: 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; 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;
}; };

View File

@ -1,4 +1,5 @@
#include "subtask_p.h" #include "subtask_p.h"
#include "container_p.h"
#include <iostream> #include <iostream>
#include <iomanip> #include <iomanip>
@ -92,6 +93,13 @@ void SubTaskPrivate::sendBackward(SubTrajectory& trajectory, const planning_scen
prevOutput()->add(ps, NULL, &trajectory); 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) PRIVATE_CLASS_IMPL(PropagatingAnyWay)
PropagatingAnyWay::PropagatingAnyWay(const std::string &name) PropagatingAnyWay::PropagatingAnyWay(const std::string &name)

View File

@ -7,16 +7,18 @@
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
class ContainerBasePrivate;
class SubTaskPrivate { class SubTaskPrivate {
friend class SubTask; friend class SubTask;
friend class SubTaskTest; // allow unit tests friend class SubTaskTest; // allow unit tests
// ContainerBase will maintain the double-linked list of interfaces friend class ContainerBasePrivate; // allow to set parent_ and it_
friend class ContainerBasePrivate;
friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage); friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
public: public:
typedef std::list<SubTask::pointer> array_type;
inline SubTaskPrivate(SubTask* me, const std::string& name) 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); SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
@ -36,14 +38,15 @@ public:
InterfacePtr output_; InterfacePtr output_;
std::list<SubTrajectory> trajectories_; std::list<SubTrajectory> trajectories_;
const InterfacePtr prevOutput() const { return predeccessor_ ? predeccessor_->output_ : InterfacePtr(); } const SubTaskPrivate* prev() const;
const InterfacePtr nextInput() const { return successor_ ? successor_->input_ : InterfacePtr(); } 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: private:
// items accessed by ContainerBasePrivate only to maintain hierarchy // items accessed by ContainerBasePrivate only to maintain hierarchy
SubTaskPrivate* parent_; ContainerBasePrivate* parent_;
SubTaskPrivate* predeccessor_; array_type::iterator it_;
SubTaskPrivate* successor_;
}; };
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage); std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);

View File

@ -74,7 +74,7 @@ Task::Task(const std::string &name)
{ {
} }
void Task::add(std::unique_ptr<SubTask> &&stage) { void Task::add(pointer &&stage) {
if (!stage) if (!stage)
throw std::runtime_error("Task::add() failed: invalid stage pointer"); throw std::runtime_error("Task::add() failed: invalid stage pointer");
if (!insert(std::move(stage))) if (!insert(std::move(stage)))

View File

@ -40,20 +40,20 @@ protected:
void TearDown() {} void TearDown() {}
// accessors for private elements of SubTaskPrivate // accessors for private elements of SubTaskPrivate
const SubTaskPrivate* parent(const SubTaskPrivate* p) const { return p->parent_; } inline const SubTaskPrivate* parent(const SubTaskPrivate* p) const { return p->parent_; }
const SubTaskPrivate* prev(const SubTaskPrivate* p) const { return p->predeccessor_; } inline const SubTaskPrivate* prev(const SubTaskPrivate* p) const { return p->prev(); }
const SubTaskPrivate* next(const SubTaskPrivate* p) const { return p->successor_; } inline const SubTaskPrivate* next(const SubTaskPrivate* p) const { return p->next(); }
void validateOrder(const SerialContainerPrivate* container, const std::initializer_list<SubTaskPrivate*> &expected) { void validateOrder(const SerialContainerPrivate* container, const std::initializer_list<SubTaskPrivate*> &expected) {
size_t num = container->children_.size(); size_t num = container->children().size();
ASSERT_TRUE(num == expected.size()) << "different list lengths"; ASSERT_TRUE(num == expected.size()) << "different list lengths";
// validate position() // validate position()
EXPECT_EQ(container->children_.begin(), container->position(-(num+1))); EXPECT_EQ(container->children().begin(), container->position(-(num+1)));
EXPECT_EQ(container->children_.end(), container->position(num)); EXPECT_EQ(container->children().end(), container->position(num));
// print order // 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(" %p", (*it)->pimpl_func());
PRINTF(" *** parent: %p ***\n", container); PRINTF(" *** parent: %p ***\n", container);
@ -62,7 +62,7 @@ protected:
const SubTaskPrivate* successor = container; const SubTaskPrivate* successor = container;
size_t pos = 0; size_t pos = 0;
auto exp_it = expected.begin(); 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(); SubTaskPrivate *child = (*it)->pimpl_func();
EXPECT_EQ(child, *exp_it) << "wrong order"; EXPECT_EQ(child, *exp_it) << "wrong order";
EXPECT_EQ(child->parent_, container) << "wrong parent"; EXPECT_EQ(child->parent_, container) << "wrong parent";