cleanup container

- removed friends + mutable
- simplified SerialContainer::canInsert()
This commit is contained in:
Robert Haschke 2017-10-14 00:07:20 +02:00
parent dc9f553ab0
commit 3cc112fe2c
5 changed files with 79 additions and 152 deletions

View File

@ -30,8 +30,6 @@ MOVEIT_CLASS_FORWARD(SubTask)
* A start or goal state for planning is essentially defined by the state of a planning scene.
*/
class InterfaceState {
friend class SubTrajectory;
public:
typedef std::deque<SubTrajectory*> SubTrajectoryList;
@ -42,12 +40,15 @@ public:
inline const SubTrajectoryList& incomingTrajectories() const { return incoming_trajectories_; }
inline const SubTrajectoryList& outgoingTrajectories() const { return outgoing_trajectories_; }
inline void addIncoming(SubTrajectory* t) { incoming_trajectories_.push_back(t); }
inline void addOutgoing(SubTrajectory* t) { outgoing_trajectories_.push_back(t); }
public:
planning_scene::PlanningSceneConstPtr state;
private:
mutable SubTrajectoryList incoming_trajectories_;
mutable SubTrajectoryList outgoing_trajectories_;
SubTrajectoryList incoming_trajectories_;
SubTrajectoryList outgoing_trajectories_;
};
@ -105,13 +106,13 @@ struct SubTrajectory {
inline void setStartState(const InterfaceState& state){
assert(begin == NULL);
begin= &state;
state.outgoing_trajectories_.push_back(this);
const_cast<InterfaceState&>(state).addOutgoing(this);
}
inline void setEndState(const InterfaceState& state){
assert(end == NULL);
end= &state;
state.incoming_trajectories_.push_back(this);
const_cast<InterfaceState&>(state).addIncoming(this);
}
// TODO: trajectories could have a name, e.g. a generator could name its solutions

View File

@ -21,8 +21,8 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before)
inline bool ContainerBasePrivate::canInsert(const SubTask &stage) const {
const SubTaskPrivate* impl = stage.pimpl();
return impl->parent_ == nullptr // re-parenting is not supported
&& impl->trajectories_.empty(); // existing trajectories would become invalid
return impl->parent() == nullptr // re-parenting is not supported
&& impl->trajectories().empty(); // existing trajectories would become invalid
}
bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &processor, int depth) const {
@ -39,9 +39,9 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&subtask,
ContainerBasePrivate::const_iterator pos) {
SubTaskPrivate *impl = subtask->pimpl();
impl->parent_ = this;
impl->it_ = children_.insert(pos, std::move(subtask));
return impl->it_;
ContainerBasePrivate::iterator it = children_.insert(pos, std::move(subtask));
impl->setHierarchy(this, it);
return it;
}
@ -53,8 +53,7 @@ PIMPL_FUNCTIONS(ContainerBase)
void ContainerBase::clear()
{
auto impl = pimpl();
impl->clear();
pimpl()->children_.clear();
}
bool ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
@ -66,22 +65,26 @@ bool ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor) const
{
auto impl = pimpl();
return impl->traverseStages(processor, 0);
pimpl()->traverseStages(processor, 0);
}
bool ContainerBase::canCompute() const
{
auto impl = pimpl();
return impl->canCompute();
pimpl()->canCompute();
}
bool ContainerBase::compute() {
auto impl = pimpl();
return impl->compute();
pimpl()->compute();
}
SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name)
: ContainerBasePrivate(me, name)
{
starts_.reset(new Interface(Interface::NotifyFunction()));
ends_.reset(new Interface(Interface::NotifyFunction()));
}
SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
InterfaceFlags f;
if (children().empty()) return f;
@ -90,31 +93,9 @@ SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
return f;
}
inline bool isConnectable(int prev_flags, int next_flags) {
return ((prev_flags & SubTaskPrivate::WRITES_NEXT_START) && (next_flags & SubTaskPrivate::READS_START)) ||
((prev_flags & SubTaskPrivate::READS_END) && (next_flags & SubTaskPrivate::WRITES_PREV_END));
}
inline bool bothWrite(SubTaskPrivate::InterfaceFlags prev_flags, SubTaskPrivate::InterfaceFlags next_flags) {
return (prev_flags.testFlag(SubTaskPrivate::WRITES_NEXT_START) && !next_flags.testFlag(SubTaskPrivate::READS_START)) &&
(next_flags.testFlag(SubTaskPrivate::WRITES_PREV_END) && !prev_flags.testFlag(SubTaskPrivate::READS_END));
}
inline bool SerialContainerPrivate::canInsert(const SubTask &stage, ContainerBasePrivate::const_iterator before) const {
if (!ContainerBasePrivate::canInsert(stage))
return false;
// check connectedness
bool at_end = (before == children().end());
const SubTaskPrivate* next = (at_end) ? this : (*before)->pimpl();
InterfaceFlags cur_flags = stage.pimpl()->announcedFlags();
InterfaceFlags next_flags = next->deducedFlags();
InterfaceFlags prev_flags = prev(before)->deducedFlags();
// Do a simple check here only. A full connectivity check requires the full pipeline to be setup
// Thus, here we reject when trying to connect to writers with each other
if (bothWrite(prev_flags, cur_flags) || bothWrite(cur_flags, next_flags))
return false;
return true;
}
@ -127,66 +108,42 @@ ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage
SubTaskPrivate *cur = stage->pimpl();
/* set pointer cache (prev_ends_ and next_starts_) of prev, current, and next stage */
if (children().empty()) { // first child inserted
setPrevEnds(cur, this->starts_);
setNextStarts(cur, this->ends_);
cur->setPrevEnds(this->starts());
cur->setNextStarts(this->ends());
} else if (at_begin) {
SubTaskPrivate *next = (*before)->pimpl();
setPrevEnds(cur, this->starts_);
setNextStarts(cur, next->starts_);
setPrevEnds(next, cur->ends_);
cur->setPrevEnds(this->starts());
cur->setNextStarts(next->starts());
next->setPrevEnds(cur->ends());
} else if (at_end) {
const SubTaskPrivate *prev = this->prev(before);
setNextStarts(prev, cur->starts_);
setPrevEnds(cur, prev->ends_);
setNextStarts(cur, this->ends_);
SubTaskPrivate *prev = (*this->prev(before))->pimpl();
prev->setNextStarts(cur->starts());
cur->setPrevEnds(prev->ends());
cur->setNextStarts(this->ends());
} else {
const SubTaskPrivate *prev = this->prev(before);
SubTaskPrivate *prev = (*this->prev(before))->pimpl();
SubTaskPrivate *next = (*before)->pimpl();
setNextStarts(prev, cur->starts_);
setPrevEnds(cur, prev->ends_);
setNextStarts(cur, next->starts_);
setPrevEnds(next, cur->ends_);
prev->setNextStarts(cur->starts());
cur->setPrevEnds(prev->ends());
cur->setNextStarts(next->starts());
next->setPrevEnds(cur->ends());
}
iterator it = ContainerBasePrivate::insert(std::move(stage), before);
}
inline const SubTaskPrivate* SerialContainerPrivate::prev(const_iterator it) const
inline ContainerBasePrivate::const_iterator SerialContainerPrivate::prev(const_iterator it) const
{
#ifndef NDEBUG
if (it != children().end()) {
SubTaskPrivate* child = (*it)->pimpl();
assert(parent(child) == this);
assert(this->it(child) == it);
}
#endif
if (it == children().begin()) return this;
return (*--it)->pimpl();
assert(it != children().cbegin());
return --it;
}
inline const SubTaskPrivate* SerialContainerPrivate::next(const_iterator it) const
inline ContainerBasePrivate::const_iterator SerialContainerPrivate::next(const_iterator it) const
{
#ifndef NDEBUG
assert(it != children().end());
SubTaskPrivate* child = (*it)->pimpl();
assert(parent(child) == this);
assert(this->it(child) == it);
#endif
if (it == --children().end()) return this;
return (*++it)->pimpl();
assert(it != children().cend());
return ++it;
}
const SubTaskPrivate *SerialContainerPrivate::prev(const SubTaskPrivate *child) const
{
return prev(it(child));
}
const SubTaskPrivate *SerialContainerPrivate::next(const SubTaskPrivate *child) const
{
return next(it(child));
}
SerialContainer::SerialContainer(SerialContainerPrivate *impl)
: ContainerBase(impl)
{}

View File

@ -8,7 +8,6 @@ namespace moveit { namespace task_constructor {
class ContainerBasePrivate : public SubTaskPrivate
{
friend class ContainerBase;
friend class BaseTest; // allow access for unit tests
public:
typedef ContainerBase::value_type value_type;
@ -21,7 +20,6 @@ public:
bool canInsert(const SubTask& stage) const;
virtual iterator insert(value_type &&subtask, const_iterator pos);
inline void clear() { children_.clear(); }
bool traverseStages(const ContainerBase::StageCallback &processor, int depth) const;
@ -29,15 +27,6 @@ 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_; }
inline void setPrevEnds(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
child->prev_ends_ = interface.get();
}
inline void setNextStarts(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
child->next_starts_ = interface.get();
}
private:
container_type children_;
@ -46,25 +35,18 @@ private:
class SerialContainerPrivate : public ContainerBasePrivate {
public:
SerialContainerPrivate(SerialContainer* me, const std::string &name)
: ContainerBasePrivate(me, name)
{
starts_.reset(new Interface(Interface::NotifyFunction()));
ends_.reset(new Interface(Interface::NotifyFunction()));
}
SerialContainerPrivate(SerialContainer* me, const std::string &name);
InterfaceFlags announcedFlags() const override;
bool canInsert(const SubTask& stage, const_iterator before) const;
inline bool canInsert(const SubTask& stage, const_iterator before) const;
virtual iterator insert(value_type &&stage, const_iterator before) override;
bool canCompute() const override;
bool compute() override;
inline const SubTaskPrivate *prev(const_iterator it) const;
inline const SubTaskPrivate *next(const_iterator it) const;
const SubTaskPrivate *prev(const SubTaskPrivate *child) const;
const SubTaskPrivate *next(const SubTaskPrivate *child) const;
private:
inline const_iterator prev(const_iterator it) const;
inline const_iterator next(const_iterator it) const;
};

View File

@ -15,8 +15,6 @@ namespace moveit { namespace task_constructor {
class ContainerBasePrivate;
class SubTaskPrivate {
friend class SubTask;
friend class BaseTest; // allow access for unit tests
friend class ContainerBasePrivate; // allow to set parent_ and it_
friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
public:
@ -45,29 +43,39 @@ public:
virtual bool canCompute() const = 0;
virtual bool compute() = 0;
public:
inline ContainerBasePrivate* parent() const { return parent_; }
inline container_type::iterator it() const { return it_; }
inline Interface* starts() const { return starts_.get(); }
inline Interface* ends() const { return ends_.get(); }
inline Interface* prevEnds() const { return prev_ends_; }
inline Interface* nextStarts() const { return next_starts_; }
inline const std::list<SubTrajectory> trajectories() const { return trajectories_; }
inline bool isConnected() const { return prev_ends_ || next_starts_; }
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
inline void setHierarchy(ContainerBasePrivate* parent, container_type::iterator it) {
parent_ = parent;
it_ = it;
}
inline void setPrevEnds(Interface * prev_ends) { prev_ends_ = prev_ends; }
inline void setNextStarts(Interface * next_starts) { next_starts_ = next_starts; }
protected:
SubTask* const me_; // associated/owning SubTask instance
const std::string name_;
InterfacePtr starts_;
InterfacePtr ends_;
inline ContainerBasePrivate* parent() const { return parent_; }
inline bool isConnected() const { return prev_ends_ || next_starts_; }
inline Interface* prevEnds() const { return prev_ends_; }
inline Interface* nextStarts() const { return next_starts_; }
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
protected:
std::list<SubTrajectory> trajectories_;
private:
// !! items accessed only by ContainerBasePrivate to maintain hierarchy !!
// !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !!
ContainerBasePrivate* parent_; // owning parent
container_type::iterator it_; // iterator into parent's children_ list referring to this
// caching the pointers to the ends_ / starts_ interface of previous / next stage
mutable Interface *prev_ends_; // interface to be used for sendBackward()
mutable Interface *next_starts_; // interface to be use for sendForward()
Interface *prev_ends_; // interface to be used for sendBackward()
Interface *next_starts_; // interface to be used for sendForward()
};
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);

View File

@ -44,9 +44,6 @@ protected:
void SetUp() {}
void TearDown() {}
// accessors for private elements of SubTaskPrivate
inline const SubTaskPrivate* parent(const SubTaskPrivate* p) const { return p->parent_; }
void validateOrder(const SerialContainerPrivate* container, const std::initializer_list<SubTaskPrivate*> &expected) {
size_t num = container->children().size();
ASSERT_TRUE(num == expected.size()) << "different list lengths";
@ -61,24 +58,15 @@ protected:
PRINTF(" *** parent: %p ***\n", container);
// validate order
const SubTaskPrivate* predeccessor = container;
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) {
SubTaskPrivate *child = (*it)->pimpl();
EXPECT_EQ(child, *exp_it) << "wrong order";
EXPECT_EQ(child->parent_, container) << "wrong parent";
EXPECT_EQ(child->parent(), container) << "wrong parent";
EXPECT_EQ(it, container->position(pos)) << "bad forward position resolution";
EXPECT_EQ(it, container->position(pos-num-1)) << "bad backward position resolution";
EXPECT_EQ(container->prev(child), predeccessor) << "wrong link to predeccessor for child " << child;
if (successor != container)
EXPECT_EQ(child, successor) << "wrong link to successor for child " << predeccessor;
// store predeccessor and successor for next round
predeccessor = child;
successor = container->next(child);
}
EXPECT_EQ(successor, container);
}
};
@ -95,39 +83,30 @@ TEST_F(BaseTest, interfaceFlags) {
TEST_F(BaseTest, serialContainer) {
SerialContainer c("serial");
SerialContainerPrivate *cp = static_cast<SerialContainerPrivate*>(c.pimpl());
SerialContainerPrivate *cp = c.pimpl();
EXPECT_TRUE(bool(cp->starts_));
EXPECT_TRUE(bool(cp->ends_));
EXPECT_EQ(parent(cp), nullptr);
EXPECT_EQ(parent(cp), nullptr);
EXPECT_TRUE(bool(cp->starts()));
EXPECT_TRUE(bool(cp->ends()));
EXPECT_EQ(cp->parent(), nullptr);
VALIDATE();
/***** inserting first stage *****/
auto g = std::make_unique<TestGenerator>();
GeneratorPrivate *gp = static_cast<GeneratorPrivate*>(g->pimpl());
GeneratorPrivate *gp = g->pimpl();
ASSERT_TRUE(c.insert(std::move(g)));
EXPECT_FALSE(g); // ownership transferred to container
VALIDATE(gp);
// inserting another generator should fail
g = std::make_unique<TestGenerator>();
EXPECT_FALSE(c.insert(std::move(g)));
/***** inserting second stage *****/
auto f = std::make_unique<TestPropagatingForward>();
PropagatingForwardPrivate *fp = static_cast<PropagatingForwardPrivate*>(f->pimpl());
PropagatingForwardPrivate *fp = f->pimpl();
ASSERT_TRUE(c.insert(std::move(f)));
EXPECT_FALSE(f); // ownership transferred to container
VALIDATE(gp, fp);
/***** inserting third stage *****/
auto f2 = std::make_unique<TestPropagatingForward>();
PropagatingForwardPrivate *fp2 = static_cast<PropagatingForwardPrivate*>(f2->pimpl());
EXPECT_FALSE(c.insert(std::move(f2), 0)); // should fail at first position
// insert @2nd position
f2 = std::make_unique<TestPropagatingForward>();
PropagatingForwardPrivate *fp2 = f2->pimpl();
ASSERT_TRUE(c.insert(std::move(f2), 1));
EXPECT_FALSE(f2); // ownership transferred to container
VALIDATE(gp, fp2, fp);