mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
29d1e44c5d
commit
06ae5ddf9c
@ -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;
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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?
|
||||
|
||||
Loading…
Reference in New Issue
Block a user