From 3cc112fe2c4820d2c6710051008856afc83b4282 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 14 Oct 2017 00:07:20 +0200 Subject: [PATCH] cleanup container - removed friends + mutable - simplified SerialContainer::canInsert() --- include/moveit_task_constructor/storage.h | 13 +-- src/container.cpp | 115 +++++++--------------- src/container_p.h | 28 +----- src/subtask_p.h | 38 ++++--- src/test/container.cpp | 37 ++----- 5 files changed, 79 insertions(+), 152 deletions(-) diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index 256b252f..981a7912 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -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 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(state).addOutgoing(this); } inline void setEndState(const InterfaceState& state){ assert(end == NULL); end= &state; - state.incoming_trajectories_.push_back(this); + const_cast(state).addIncoming(this); } // TODO: trajectories could have a name, e.g. a generator could name its solutions diff --git a/src/container.cpp b/src/container.cpp index d95f19d0..3251df1f 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -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) {} diff --git a/src/container_p.h b/src/container_p.h index 4b7e3328..9840398e 100644 --- a/src/container_p.h +++ b/src/container_p.h @@ -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; }; diff --git a/src/subtask_p.h b/src/subtask_p.h index 72264657..1219f093 100644 --- a/src/subtask_p.h +++ b/src/subtask_p.h @@ -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 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 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); diff --git a/src/test/container.cpp b/src/test/container.cpp index b41f6580..d05bac84 100644 --- a/src/test/container.cpp +++ b/src/test/container.cpp @@ -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 &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(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(); - GeneratorPrivate *gp = static_cast(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(); - EXPECT_FALSE(c.insert(std::move(g))); - /***** inserting second stage *****/ auto f = std::make_unique(); - PropagatingForwardPrivate *fp = static_cast(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(); - PropagatingForwardPrivate *fp2 = static_cast(f2->pimpl()); - EXPECT_FALSE(c.insert(std::move(f2), 0)); // should fail at first position - - // insert @2nd position - f2 = std::make_unique(); + PropagatingForwardPrivate *fp2 = f2->pimpl(); ASSERT_TRUE(c.insert(std::move(f2), 1)); EXPECT_FALSE(f2); // ownership transferred to container VALIDATE(gp, fp2, fp);