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:
|
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;
|
||||||
|
|||||||
@ -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,
|
||||||
|
|||||||
@ -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();
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -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)
|
||||||
|
|||||||
@ -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);
|
||||||
|
|
||||||
|
|||||||
@ -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)))
|
||||||
|
|||||||
@ -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";
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user