Recursively re-enable states when matching an ARMED state

This commit is contained in:
Robert Haschke 2021-11-20 14:43:37 +01:00
parent 3c4ef68dbe
commit ab9af6a0fa
2 changed files with 14 additions and 11 deletions

View File

@ -76,6 +76,7 @@ namespace task_constructor {
class ContainerBasePrivate : public StagePrivate
{
friend class ContainerBase;
friend class ConnectingPrivate; // needs to call protected setStatus()
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
public:

View File

@ -709,23 +709,25 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
return StatePair(second, first);
}
template <Interface::Direction other>
template <Interface::Direction dir>
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
if (updated) { // many pairs might be affected: resort
pending.sort();
} else { // new state: insert all pairs with other interface
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
InterfacePtr other_interface = pullInterface(other);
auto parent_pimpl = parent()->pimpl();
InterfacePtr other_interface = pullInterface(dir);
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
if (static_cast<Connecting*>(me_)->compatible(*it, *oit)) {
// re-enable the opposing state oit if its status is ARMED,
// but don't re-enable states that are marked DISABLED
// https://github.com/ros-planning/moveit_task_constructor/pull/221
if (oit->priority().status() == InterfaceState::Status::ARMED)
oit->owner()->updatePriority(oit,
InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED));
pending.insert(make_pair<other>(it, oit));
}
if (!static_cast<Connecting*>(me_)->compatible(*it, *oit))
continue;
// re-enable the opposing state oit (and its associated solution branch) if its status is ARMED
// 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);
// Remember all pending states, regardless of their status!
pending.insert(make_pair<dir>(it, oit));
}
}
// std::cerr << name_ << ": ";