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 class ContainerBasePrivate : public StagePrivate
{ {
friend class ContainerBase; friend class ContainerBase;
friend class ConnectingPrivate; // needs to call protected setStatus()
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs); friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
public: public:

View File

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