mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Fix test Pruning.NoPruningIfAlternativesExist
This commit is contained in:
parent
06ae5ddf9c
commit
52dc494525
@ -148,9 +148,10 @@ protected:
|
|||||||
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Set ENABLED / PRUNED status of the solution tree starting from s into given direction
|
/// Set ENABLED/PRUNED/FAILED status of a solution branch starting from target into the given direction
|
||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
void setStatus(const InterfaceState* s, InterfaceState::Status status);
|
void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
|
||||||
|
InterfaceState::Status status);
|
||||||
|
|
||||||
/// copy external_state to a child's interface and remember the link in internal_external map
|
/// copy external_state to a child's interface and remember the link in internal_external map
|
||||||
template <Interface::Direction>
|
template <Interface::Direction>
|
||||||
|
|||||||
@ -118,32 +118,32 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
|
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
|
||||||
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
|
setStatus<Interface::BACKWARD>(nullptr, nullptr, from, InterfaceState::Status::FAILED);
|
||||||
break;
|
break;
|
||||||
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
|
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
|
||||||
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
|
setStatus<Interface::FORWARD>(nullptr, nullptr, to, InterfaceState::Status::FAILED);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case CONNECT:
|
case CONNECT:
|
||||||
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
|
setStatus<Interface::BACKWARD>(&child, to, from, InterfaceState::Status::FAILED);
|
||||||
// only prune if there are no opposite pending states
|
setStatus<Interface::FORWARD>(&child, from, to, InterfaceState::Status::FAILED);
|
||||||
auto cimpl = conn->pimpl();
|
|
||||||
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to, from))
|
|
||||||
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
|
|
||||||
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from, to))
|
|
||||||
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
|
|
||||||
} else { // other CONNECT-like stages, e.g. containers are always pruned
|
|
||||||
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
|
|
||||||
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
// printChildrenInterfaces(*this, false, child);
|
// printChildrenInterfaces(*this, false, child);
|
||||||
}
|
}
|
||||||
|
|
||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) {
|
void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
|
||||||
if (s->priority().status() == status)
|
InterfaceState::Status status) {
|
||||||
|
if (status != InterfaceState::Status::ENABLED && creator) {
|
||||||
|
if (const auto* conn = dynamic_cast<const Connecting*>(creator)) {
|
||||||
|
auto cimpl = conn->pimpl();
|
||||||
|
// if creator is a Connecting stage and target has enabled opposite states (other than source)
|
||||||
|
if (cimpl->hasPendingOpposites<dir>(source, target))
|
||||||
|
return; // don't prune
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (target->priority().status() == status)
|
||||||
return; // nothing changing
|
return; // nothing changing
|
||||||
|
|
||||||
// Skip disabling the state, if there are alternative enabled solutions
|
// Skip disabling the state, if there are alternative enabled solutions
|
||||||
@ -151,18 +151,18 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St
|
|||||||
auto solution_is_enabled = [](auto&& solution) {
|
auto solution_is_enabled = [](auto&& solution) {
|
||||||
return state<opposite<dir>()>(*solution)->priority().enabled();
|
return state<opposite<dir>()>(*solution)->priority().enabled();
|
||||||
};
|
};
|
||||||
const auto& alternatives = trajectories<opposite<dir>()>(*s);
|
const auto& alternatives = trajectories<opposite<dir>()>(*target);
|
||||||
auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled);
|
auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled);
|
||||||
if (alternative_path != alternatives.cend())
|
if (alternative_path != alternatives.cend())
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// actually enable/disable the state
|
// actually enable/disable the state
|
||||||
const_cast<InterfaceState*>(s)->updateStatus(status);
|
const_cast<InterfaceState*>(target)->updateStatus(status);
|
||||||
|
|
||||||
// if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface
|
// if possible (i.e. if target has an external counterpart), escalate setStatus to external interface
|
||||||
if (parent() && trajectories<dir>(*s).empty()) {
|
if (parent() && trajectories<dir>(*target).empty()) {
|
||||||
auto external{ internalToExternalMap().find(s) };
|
auto external{ internalToExternalMap().find(target) };
|
||||||
if (external != internalToExternalMap().end()) { // do we have an external state?
|
if (external != internalToExternalMap().end()) { // do we have an external state?
|
||||||
// only escalate if there is no other *enabled* internal state connected to the same external one
|
// only escalate if there is no other *enabled* internal state connected to the same external one
|
||||||
// all internal states linked to external
|
// all internal states linked to external
|
||||||
@ -170,7 +170,7 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St
|
|||||||
auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); };
|
auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); };
|
||||||
auto other_path{ std::find_if(internals.first, internals.second, is_enabled) };
|
auto other_path{ std::find_if(internals.first, internals.second, is_enabled) };
|
||||||
if (other_path == internals.second)
|
if (other_path == internals.second)
|
||||||
parent()->pimpl()->setStatus<dir>(external->get<EXTERNAL>(), status);
|
parent()->pimpl()->setStatus<dir>(nullptr, nullptr, external->get<EXTERNAL>(), status);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -185,18 +185,16 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St
|
|||||||
status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED
|
status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED
|
||||||
|
|
||||||
// traverse solution tree
|
// traverse solution tree
|
||||||
for (const SolutionBase* successor : trajectories<dir>(*s))
|
for (const SolutionBase* successor : trajectories<dir>(*target))
|
||||||
setStatus<dir>(state<dir>(*successor), status);
|
setStatus<dir>(successor->creator(), target, state<dir>(*successor), status);
|
||||||
}
|
}
|
||||||
template void ContainerBasePrivate::setStatus<Interface::FORWARD>(const InterfaceState*, InterfaceState::Status);
|
|
||||||
template void ContainerBasePrivate::setStatus<Interface::BACKWARD>(const InterfaceState*, InterfaceState::Status);
|
|
||||||
|
|
||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
|
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
|
||||||
if (updated) {
|
if (updated) { // propagate external state update to internal copies
|
||||||
auto internals{ externalToInternalMap().equal_range(&*external) };
|
auto internals{ externalToInternalMap().equal_range(&*external) };
|
||||||
for (auto& i = internals.first; i != internals.second; ++i) {
|
for (auto& i = internals.first; i != internals.second; ++i) {
|
||||||
setStatus<dir>(i->second, external->priority().status());
|
setStatus<dir>(nullptr, nullptr, i->second, external->priority().status());
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -46,7 +46,7 @@ TEST_F(Pruning, PruningMultiForward) {
|
|||||||
|
|
||||||
// The 2nd failing FW attempt would prune the path through CON,
|
// The 2nd failing FW attempt would prune the path through CON,
|
||||||
// but shouldn't because there exist two more GEN2 solutions
|
// but shouldn't because there exist two more GEN2 solutions
|
||||||
TEST_F(Pruning, DISABLED_NoPruningIfAlternativesExist) {
|
TEST_F(Pruning, NoPruningIfAlternativesExist) {
|
||||||
add(t, new GeneratorMockup(PredefinedCosts({ 0.0 })));
|
add(t, new GeneratorMockup(PredefinedCosts({ 0.0 })));
|
||||||
add(t, new ConnectMockup());
|
add(t, new ConnectMockup());
|
||||||
add(t, new GeneratorMockup(std::list<double>{ 0, 10, 20, 30 }, 2));
|
add(t, new GeneratorMockup(std::list<double>{ 0, 10, 20, 30 }, 2));
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user