mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Propagate status across Connecting gap
Not only propagate updates along solution paths, but also bridge the gap of a `Connecting` stage. - If a state becomes enabled, re-enable opposite `ARMED` states as well. - If a state becomes pruned, also prune opposite states if they don't have alternatives. - Make sure that we don't run into a recursive update loop by disabling notify() callbacks.
This commit is contained in:
parent
3648db43ed
commit
a31e52dd53
@ -200,6 +200,18 @@ public:
|
|||||||
BACKWARD,
|
BACKWARD,
|
||||||
};
|
};
|
||||||
using NotifyFunction = std::function<void(iterator, bool)>;
|
using NotifyFunction = std::function<void(iterator, bool)>;
|
||||||
|
|
||||||
|
class DisableNotify
|
||||||
|
{
|
||||||
|
Interface& if_;
|
||||||
|
Interface::NotifyFunction old_;
|
||||||
|
|
||||||
|
public:
|
||||||
|
DisableNotify(Interface& i) : if_(i) { old_.swap(if_.notify_); }
|
||||||
|
~DisableNotify() { old_.swap(if_.notify_); }
|
||||||
|
};
|
||||||
|
friend class DisableNotify;
|
||||||
|
|
||||||
Interface(const NotifyFunction& notify = NotifyFunction());
|
Interface(const NotifyFunction& notify = NotifyFunction());
|
||||||
|
|
||||||
/// add a new InterfaceState
|
/// add a new InterfaceState
|
||||||
@ -210,9 +222,10 @@ public:
|
|||||||
|
|
||||||
/// update state's priority (and call notify_ if it really has changed)
|
/// update state's priority (and call notify_ if it really has changed)
|
||||||
void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority);
|
void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority);
|
||||||
|
inline bool notifyEnabled() const { return static_cast<bool>(notify_); }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const NotifyFunction notify_;
|
NotifyFunction notify_;
|
||||||
|
|
||||||
// restrict access to some functions to ensure consistency
|
// restrict access to some functions to ensure consistency
|
||||||
// (we need to set/unset InterfaceState::owner_)
|
// (we need to set/unset InterfaceState::owner_)
|
||||||
|
|||||||
@ -709,13 +709,44 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
|
|||||||
return StatePair(second, first);
|
return StatePair(second, first);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// TODO: bool updated -> uint_8 updated (bitfield of PRIORITY | STATUS)
|
||||||
template <Interface::Direction dir>
|
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
|
auto parent_pimpl = parent()->pimpl();
|
||||||
|
Interface::DisableNotify disable_source_interface(*pullInterface(dir));
|
||||||
|
if (updated) {
|
||||||
|
if (pullInterface(opposite<dir>())->notifyEnabled()) // suppress recursive loop
|
||||||
|
{
|
||||||
|
// If status has changed, propagate the update to the opposite side
|
||||||
|
auto status = it->priority().status();
|
||||||
|
if (status == InterfaceState::Status::PRUNED) // PRUNED becomes ARMED on opposite side
|
||||||
|
status = InterfaceState::Status::ARMED; // (only for pending state pairs)
|
||||||
|
|
||||||
|
for (const auto& candidate : this->pending) {
|
||||||
|
if (std::get<opposite<dir>()>(candidate) != it) // only consider pairs with source state == state
|
||||||
|
continue;
|
||||||
|
auto oit = std::get<dir>(candidate); // opposite target state
|
||||||
|
auto ostatus = oit->priority().status();
|
||||||
|
if (ostatus != status) {
|
||||||
|
if (status != InterfaceState::Status::ENABLED) {
|
||||||
|
// quicker check for hasPendingOpposites(): search in it->owner() for an enabled alternative
|
||||||
|
bool cancel = false; // if found, cancel propagation of new status
|
||||||
|
for (const auto alternative : *it->owner())
|
||||||
|
if ((cancel = alternative->priority().enabled()))
|
||||||
|
break;
|
||||||
|
if (cancel)
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
// pass creator=nullptr to skip hasPendingOpposites() check
|
||||||
|
parent_pimpl->setStatus<opposite<dir>()>(nullptr, nullptr, &*oit, status);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// many pairs will have changed priorities: resort pending list
|
||||||
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?
|
||||||
auto parent_pimpl = parent()->pimpl();
|
|
||||||
InterfacePtr other_interface = pullInterface(dir);
|
InterfacePtr other_interface = pullInterface(dir);
|
||||||
bool have_enabled_opposites = false;
|
bool have_enabled_opposites = false;
|
||||||
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) {
|
||||||
|
|||||||
@ -91,6 +91,7 @@ TYPED_TEST(PruningContainerTests, ConnectReactivatesPrunedPaths) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(Pruning, ConnectConnectForward) {
|
TEST_F(Pruning, ConnectConnectForward) {
|
||||||
|
add(t, new BackwardMockup());
|
||||||
add(t, new GeneratorMockup());
|
add(t, new GeneratorMockup());
|
||||||
auto c1 = add(t, new ConnectMockup({ INF, 0, 0 })); // 1st attempt is a failue
|
auto c1 = add(t, new ConnectMockup({ INF, 0, 0 })); // 1st attempt is a failue
|
||||||
add(t, new GeneratorMockup({ 0, 10, 20 }));
|
add(t, new GeneratorMockup({ 0, 10, 20 }));
|
||||||
@ -112,6 +113,7 @@ TEST_F(Pruning, ConnectConnectForward) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(Pruning, ConnectConnectBackward) {
|
TEST_F(Pruning, ConnectConnectBackward) {
|
||||||
|
add(t, new BackwardMockup());
|
||||||
add(t, new GeneratorMockup({ 1, 2, 3 }));
|
add(t, new GeneratorMockup({ 1, 2, 3 }));
|
||||||
auto c1 = add(t, new ConnectMockup());
|
auto c1 = add(t, new ConnectMockup());
|
||||||
add(t, new BackwardMockup());
|
add(t, new BackwardMockup());
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user