mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
removed Stage::validate()
... only was checking for implies(a, a) which is always true
This commit is contained in:
parent
104e52eb48
commit
e4ad7a0753
@ -107,8 +107,14 @@ int main(int argc, char** argv){
|
|||||||
t.add(std::move(move));
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
t.plan();
|
try {
|
||||||
t.publishAllSolutions();
|
t.plan();
|
||||||
|
t.publishAllSolutions();
|
||||||
|
}
|
||||||
|
catch (const InitStageException &e) {
|
||||||
|
std::cerr << e;
|
||||||
|
return EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -79,10 +79,6 @@ public:
|
|||||||
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
|
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
|
||||||
inline InterfaceConstPtr nextStarts() const { return next_starts_.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
|
/// the following methods should be called only by a container
|
||||||
/// to setup the connection structure of their children
|
/// to setup the connection structure of their children
|
||||||
inline void setHierarchy(ContainerBase* parent, container_type::iterator it) {
|
inline void setHierarchy(ContainerBase* parent, container_type::iterator it) {
|
||||||
|
|||||||
@ -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)
|
if (errors)
|
||||||
throw 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 there are no children, there is nothing to connect
|
||||||
if (!impl->children().empty()) {
|
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
|
// last stage sends forward to pending_forward_
|
||||||
auto cur = impl->children().begin();
|
auto last = --impl->children().end();
|
||||||
Stage* child = cur->get();
|
(*last)->pimpl()->setNextStarts(impl->pending_forward_);
|
||||||
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);
|
|
||||||
}));
|
|
||||||
|
|
||||||
auto last = --impl->children().end();
|
auto cur = start;
|
||||||
child = last->get();
|
auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage
|
||||||
if (child->pimpl()->ends())
|
if (prev != last) {// we have more than one children
|
||||||
impl->ends_.reset(new Interface([impl, child](const Interface::iterator& external){
|
auto next = cur; ++next; // next points to 3rd stage (or end)
|
||||||
// new external state in our ends_ interface is copied to last child
|
for (; cur != last; ++prev, ++cur, ++next) {
|
||||||
impl->copyState(*external, *child, false);
|
impl->connect(**prev, **cur);
|
||||||
}));
|
impl->connect(**cur, **next);
|
||||||
|
}
|
||||||
/*** connect children ***/
|
// finally connect last == cur and prev stage
|
||||||
// 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) {
|
|
||||||
impl->connect(**prev, **cur);
|
impl->connect(**prev, **cur);
|
||||||
impl->connect(**cur, **next);
|
|
||||||
}
|
}
|
||||||
// finally connect last == cur and prev stage
|
|
||||||
impl->connect(**prev, **cur);
|
|
||||||
}
|
|
||||||
|
|
||||||
// recursively init + validate all children
|
// recursively init + validate all children
|
||||||
// this needs to be done *after* initializing the connections
|
// this needs to be done *after* initializing the connections
|
||||||
ContainerBase::init(scene);
|
ContainerBase::init(scene);
|
||||||
|
|
||||||
// after initializing children, they might have changed their mind about reading...
|
// initialize starts_ and ends_ interfaces
|
||||||
if (!impl->children().front()->pimpl()->starts())
|
Stage* child = start->get();
|
||||||
impl->starts_.reset();
|
if (child->pimpl()->starts())
|
||||||
if (!impl->children().back()->pimpl()->ends())
|
impl->starts_.reset(new Interface([impl, child](const Interface::iterator& external){
|
||||||
impl->ends_.reset();
|
// 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 {
|
} else {
|
||||||
|
errors.push_back(*this, "no children");
|
||||||
// no children -> no reading
|
// no children -> no reading
|
||||||
impl->starts_.reset();
|
impl->starts_.reset();
|
||||||
impl->ends_.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)
|
if (errors)
|
||||||
|
|||||||
@ -82,26 +82,10 @@ InterfaceFlags StagePrivate::interfaceFlags() const
|
|||||||
if (starts()) f |= READS_START;
|
if (starts()) f |= READS_START;
|
||||||
if (ends()) f |= READS_END;
|
if (ends()) f |= READS_END;
|
||||||
if (prevEnds()) f |= WRITES_PREV_END;
|
if (prevEnds()) f |= WRITES_PREV_END;
|
||||||
if (nextStarts()) f |= WRITES_NEXT_START;
|
if (nextStarts()) f |= WRITES_NEXT_START;
|
||||||
return f;
|
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)
|
void StagePrivate::newSolution(const SolutionBase &solution)
|
||||||
{
|
{
|
||||||
for (const auto& cb : solution_cbs_)
|
for (const auto& cb : solution_cbs_)
|
||||||
|
|||||||
@ -54,10 +54,14 @@ protected:
|
|||||||
TEST_F(BaseTest, serialContainer) {
|
TEST_F(BaseTest, serialContainer) {
|
||||||
SerialContainer c("serial");
|
SerialContainer c("serial");
|
||||||
SerialContainerPrivate *cp = c.pimpl();
|
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;
|
planning_scene::PlanningScenePtr scene;
|
||||||
|
|
||||||
EXPECT_EQ(cp->parent(), nullptr);
|
EXPECT_EQ(cp->parent(), nullptr);
|
||||||
c.init(scene);
|
EXPECT_THROW(c.init(scene), InitStageException);
|
||||||
VALIDATE();
|
VALIDATE();
|
||||||
|
|
||||||
/***** inserting first stage *****/
|
/***** inserting first stage *****/
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user