diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index 0e343559..adc1e8bf 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -107,8 +107,14 @@ int main(int argc, char** argv){ t.add(std::move(move)); } - t.plan(); - t.publishAllSolutions(); + try { + t.plan(); + t.publishAllSolutions(); + } + catch (const InitStageException &e) { + std::cerr << e; + return EINVAL; + } return 0; } diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 2a37a2a1..c6c4e504 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -79,10 +79,6 @@ public: inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); } inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); } - /// validate correct configuration of this stage - /// should be only called by containers' init() method - void validate() const; - /// the following methods should be called only by a container /// to setup the connection structure of their children inline void setHierarchy(ContainerBase* parent, container_type::iterator it) { diff --git a/core/src/container.cpp b/core/src/container.cpp index 12b10658..c1764053 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -188,22 +188,6 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) } } - // validate connectivity of children - for (auto& child : children) { - try { - child->pimpl()->validate(); - } catch (InitStageException &e) { - errors.append(e); - } - } - - // validate connectivity of this - try { - pimpl()->validate(); - } catch (InitStageException &e) { - errors.append(e); - } - if (errors) throw errors; } @@ -344,63 +328,56 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) // if there are no children, there is nothing to connect if (!impl->children().empty()) { + /*** connect children ***/ + // first stage sends backward to pending_backward_ + auto start = impl->children().begin(); + (*start)->pimpl()->setPrevEnds(impl->pending_backward_); - // initialize starts_ and ends_ interfaces - auto cur = impl->children().begin(); - Stage* child = cur->get(); - if (child->pimpl()->starts()) - impl->starts_.reset(new Interface([impl, child](const Interface::iterator& external){ - // new external state in our starts_ interface is copied to first child - impl->copyState(*external, *child, true); - })); + // last stage sends forward to pending_forward_ + auto last = --impl->children().end(); + (*last)->pimpl()->setNextStarts(impl->pending_forward_); - auto last = --impl->children().end(); - child = last->get(); - if (child->pimpl()->ends()) - impl->ends_.reset(new Interface([impl, child](const Interface::iterator& external){ - // new external state in our ends_ interface is copied to last child - impl->copyState(*external, *child, false); - })); - - /*** connect children ***/ - // first stage sends backward to pending_backward_ - (*cur)->pimpl()->setPrevEnds(impl->pending_backward_); - - // last stage sends forward to pending_forward_ - (*last)->pimpl()->setNextStarts(impl->pending_forward_); - - auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage - if (prev != last) {// we have more than one children - auto next = cur; ++next; // next points to 3rd stage (or end) - for (; cur != last; ++prev, ++cur, ++next) { + auto cur = start; + auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage + if (prev != last) {// we have more than one children + auto next = cur; ++next; // next points to 3rd stage (or end) + for (; cur != last; ++prev, ++cur, ++next) { + impl->connect(**prev, **cur); + impl->connect(**cur, **next); + } + // finally connect last == cur and prev stage impl->connect(**prev, **cur); - impl->connect(**cur, **next); } - // finally connect last == cur and prev stage - impl->connect(**prev, **cur); - } - // recursively init + validate all children - // this needs to be done *after* initializing the connections - ContainerBase::init(scene); + // recursively init + validate all children + // this needs to be done *after* initializing the connections + ContainerBase::init(scene); - // after initializing children, they might have changed their mind about reading... - if (!impl->children().front()->pimpl()->starts()) - impl->starts_.reset(); - if (!impl->children().back()->pimpl()->ends()) - impl->ends_.reset(); + // initialize starts_ and ends_ interfaces + Stage* child = start->get(); + if (child->pimpl()->starts()) + impl->starts_.reset(new Interface([impl, child](const Interface::iterator& external){ + // new external state in our starts_ interface is copied to first child + impl->copyState(*external, *child, true); + })); + child = last->get(); + if (child->pimpl()->ends()) + impl->ends_.reset(new Interface([impl, child](const Interface::iterator& external){ + // new external state in our ends_ interface is copied to last child + impl->copyState(*external, *child, false); + })); + + // validate connectivity of this + if (!impl->nextStarts()) + errors.push_back(*this, "cannot sendForward()"); + if (!impl->prevEnds()) + errors.push_back(*this, "cannot sendBackward()"); } else { + errors.push_back(*this, "no children"); // no children -> no reading impl->starts_.reset(); impl->ends_.reset(); - - // validate connectivity of this (would have been done in ContainerBase::init) - try { - pimpl()->validate(); - } catch (InitStageException &e) { - errors.append(e); - } } if (errors) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index c6cebf41..3d361eb5 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -82,26 +82,10 @@ InterfaceFlags StagePrivate::interfaceFlags() const if (starts()) f |= READS_START; if (ends()) f |= READS_END; if (prevEnds()) f |= WRITES_PREV_END; - if (nextStarts()) f |= WRITES_NEXT_START; + if (nextStarts()) f |= WRITES_NEXT_START; return f; } -inline bool implies(bool p, bool q) { return !p || q; } -void StagePrivate::validate() const { - InitStageException errors; - - InterfaceFlags f = interfaceFlags(); - // validate that sendForward() will succeed - if (!implies(f & WRITES_NEXT_START, bool(nextStarts()))) - errors.push_back(*me_, "sends forward, but next stage cannot receive"); - - // validate that sendBackward() will succeed - if (!implies(f & WRITES_PREV_END, bool(prevEnds()))) - errors.push_back(*me_, "sends backward, but previous stage cannot receive"); - - if (errors) throw errors; -} - void StagePrivate::newSolution(const SolutionBase &solution) { for (const auto& cb : solution_cbs_) diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 46e22b61..d179c136 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -54,10 +54,14 @@ protected: TEST_F(BaseTest, serialContainer) { SerialContainer c("serial"); SerialContainerPrivate *cp = c.pimpl(); + // pretend, that the container is connected + InterfacePtr dummy(new Interface(Interface::NotifyFunction())); + cp->setNextStarts(dummy); + cp->setPrevEnds(dummy); planning_scene::PlanningScenePtr scene; EXPECT_EQ(cp->parent(), nullptr); - c.init(scene); + EXPECT_THROW(c.init(scene), InitStageException); VALIDATE(); /***** inserting first stage *****/