Fix hasPendingOpposites()

- Switch directions: FORWARD <-> BACKWARD to make the function reusable for status propagation.
- We need to ignore the source state when looking for opposite states of the target state.
  Thus add both, source and target state arguments.
This commit is contained in:
Robert Haschke 2021-11-19 23:02:59 +01:00
parent 29d1e44c5d
commit 06ae5ddf9c
3 changed files with 17 additions and 17 deletions

View File

@ -330,7 +330,7 @@ public:
// Check whether there are pending feasible states that could connect to source
template <Interface::Direction dir>
bool hasPendingOpposites(const InterfaceState* source) const;
bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const;
std::ostream& printPendingPairs(std::ostream& os = std::cerr) const;

View File

@ -118,25 +118,23 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState
break;
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch");
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
break;
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch");
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
break;
case CONNECT:
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
// only prune if there are no opposite pending states
auto cimpl = conn->pimpl();
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from)) {
ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch");
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to, from))
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
}
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to)) {
ROS_DEBUG_STREAM_NAMED("Pruning", "prune forward branch");
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from, to))
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
}
} else { // other CONNECT-like stages, e.g. containers are always pruned
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
}
break;
}

View File

@ -733,17 +733,17 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
// std::cerr << std::endl;
}
// Check whether there are pending feasible states that could connect to source.
// If not, we exhausted all solution candidates for source and thus should mark it as failure.
// Check whether there are pending feasible states (other than source) that could connect to target.
// If not, we exhausted all solution candidates for target and thus should mark it as failure.
template <Interface::Direction dir>
inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const {
inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const {
for (const auto& candidate : this->pending) {
static_assert(Interface::FORWARD == 0 && Interface::BACKWARD == 1,
"This code assumes FORWARD=0, BACKWARD=1. Don't change their order!");
const auto src = std::get<dir>(candidate);
const auto tgt = std::get<opposite<dir>()>(candidate);
const InterfaceState* src = &*std::get<dir>(candidate);
const InterfaceState* tgt = &*std::get<opposite<dir>()>(candidate);
if (&*src == source && tgt->priority().enabled())
if (tgt == target && src != source && src->priority().enabled())
return true;
// early stopping when only infeasible pairs are to come
@ -753,8 +753,10 @@ inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source)
return false;
}
// explicitly instantiate templates for both directions
template bool ConnectingPrivate::hasPendingOpposites<Interface::FORWARD>(const InterfaceState* source) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const InterfaceState* source) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::FORWARD>(const InterfaceState* start,
const InterfaceState* end) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const InterfaceState* end,
const InterfaceState* start) const;
bool ConnectingPrivate::canCompute() const {
// Do we still have feasible pending state pairs?