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,
|
||||
};
|
||||
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());
|
||||
|
||||
/// add a new InterfaceState
|
||||
@ -210,9 +222,10 @@ public:
|
||||
|
||||
/// update state's priority (and call notify_ if it really has changed)
|
||||
void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority);
|
||||
inline bool notifyEnabled() const { return static_cast<bool>(notify_); }
|
||||
|
||||
private:
|
||||
const NotifyFunction notify_;
|
||||
NotifyFunction notify_;
|
||||
|
||||
// restrict access to some functions to ensure consistency
|
||||
// (we need to set/unset InterfaceState::owner_)
|
||||
|
||||
@ -709,13 +709,44 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
|
||||
return StatePair(second, first);
|
||||
}
|
||||
|
||||
// TODO: bool updated -> uint_8 updated (bitfield of PRIORITY | STATUS)
|
||||
template <Interface::Direction dir>
|
||||
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();
|
||||
} else { // new state: insert all pairs with other interface
|
||||
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
|
||||
auto parent_pimpl = parent()->pimpl();
|
||||
InterfacePtr other_interface = pullInterface(dir);
|
||||
bool have_enabled_opposites = false;
|
||||
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) {
|
||||
add(t, new BackwardMockup());
|
||||
add(t, new GeneratorMockup());
|
||||
auto c1 = add(t, new ConnectMockup({ INF, 0, 0 })); // 1st attempt is a failue
|
||||
add(t, new GeneratorMockup({ 0, 10, 20 }));
|
||||
@ -112,6 +113,7 @@ TEST_F(Pruning, ConnectConnectForward) {
|
||||
}
|
||||
|
||||
TEST_F(Pruning, ConnectConnectBackward) {
|
||||
add(t, new BackwardMockup());
|
||||
add(t, new GeneratorMockup({ 1, 2, 3 }));
|
||||
auto c1 = add(t, new ConnectMockup());
|
||||
add(t, new BackwardMockup());
|
||||
|
||||
Loading…
Reference in New Issue
Block a user