mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
bb06eda33c
commit
11b6dd2efd
@ -9,7 +9,7 @@ class ContainerBase : public SubTask
|
||||
{
|
||||
public:
|
||||
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 std::vector<SubTrajectory*>&)> SolutionCallback;
|
||||
|
||||
@ -34,17 +34,16 @@ class InterfaceState;
|
||||
typedef std::pair<const InterfaceState&, const InterfaceState&> 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<SubTask> pointer;
|
||||
enum InterfaceFlag {
|
||||
READS_INPUT = 0x01,
|
||||
READS_OUTPUT = 0x02,
|
||||
|
||||
@ -28,7 +28,7 @@ public:
|
||||
|
||||
Task(const std::string &name = std::string());
|
||||
|
||||
void add(std::unique_ptr<SubTask> &&stage);
|
||||
void add(SubTask::pointer &&stage);
|
||||
using SerialContainer::clear;
|
||||
|
||||
bool plan();
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -9,25 +9,19 @@ class ContainerBasePrivate : public SubTaskPrivate
|
||||
{
|
||||
public:
|
||||
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::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;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -1,4 +1,5 @@
|
||||
#include "subtask_p.h"
|
||||
#include "container_p.h"
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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<SubTask::pointer> 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<SubTrajectory> 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);
|
||||
|
||||
|
||||
@ -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)
|
||||
throw std::runtime_error("Task::add() failed: invalid stage pointer");
|
||||
if (!insert(std::move(stage)))
|
||||
|
||||
@ -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<SubTaskPrivate*> &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";
|
||||
|
||||
Loading…
Reference in New Issue
Block a user