mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Recursively re-enable states when matching an ARMED state
This commit is contained in:
parent
3c4ef68dbe
commit
ab9af6a0fa
@ -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:
|
||||||
|
|||||||
@ -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_ << ": ";
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user