Recursively prune new CONNECT state if there is no enabled opposite

This also requires to drop the assertion in SerialContainer::onNewSolution()
that new solutions will have enabled start+end states (a CONNECT stage's solution might not).
This commit is contained in:
Robert Haschke 2021-11-20 15:02:35 +01:00
parent ab9af6a0fa
commit ffb8598363
2 changed files with 6 additions and 4 deletions

View File

@ -443,10 +443,6 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
// failures should never trigger this callback
assert(!current.isFailure());
// states of solution must be active, otherwise this would not have been computed
assert(current.start()->priority().enabled());
assert(current.end()->priority().enabled());
auto impl = pimpl();
const Stage* creator = current.creator();
auto& children = impl->children();

View File

@ -717,6 +717,7 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
auto parent_pimpl = parent()->pimpl();
InterfacePtr other_interface = pullInterface(dir);
bool have_enabled_opposites = false;
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
if (!static_cast<Connecting*>(me_)->compatible(*it, *oit))
continue;
@ -725,10 +726,15 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
// https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202
if (oit->priority().status() == InterfaceState::Status::ARMED)
parent_pimpl->setStatus<opposite<dir>()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED);
if (oit->priority().enabled())
have_enabled_opposites = true;
// Remember all pending states, regardless of their status!
pending.insert(make_pair<dir>(it, oit));
}
if (!have_enabled_opposites) // prune new state and associated branch if necessary
// pass creator=nullptr to skip hasPendingOpposites() check as we did this here already
parent_pimpl->setStatus<dir>(nullptr, nullptr, &*it, InterfaceState::Status::ARMED);
}
// std::cerr << name_ << ": ";
// printPendingPairs(std::cerr);