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));
|
||||
}
|
||||
|
||||
try {
|
||||
t.plan();
|
||||
t.publishAllSolutions();
|
||||
}
|
||||
catch (const InitStageException &e) {
|
||||
std::cerr << e;
|
||||
return EINVAL;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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,31 +328,16 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
|
||||
// if there are no children, there is nothing to connect
|
||||
if (!impl->children().empty()) {
|
||||
|
||||
// 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);
|
||||
}));
|
||||
|
||||
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_);
|
||||
auto start = impl->children().begin();
|
||||
(*start)->pimpl()->setPrevEnds(impl->pending_backward_);
|
||||
|
||||
// last stage sends forward to pending_forward_
|
||||
auto last = --impl->children().end();
|
||||
(*last)->pimpl()->setNextStarts(impl->pending_forward_);
|
||||
|
||||
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)
|
||||
@ -384,23 +353,31 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
// 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)
|
||||
|
||||
@ -86,22 +86,6 @@ InterfaceFlags StagePrivate::interfaceFlags() const
|
||||
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_)
|
||||
|
||||
@ -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 *****/
|
||||
|
||||
Loading…
Reference in New Issue
Block a user