mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
resolve PruningMultiForward test
Every InterfaceState along the partial solution has to be disabled for pruning, not just the ends that are currently relevant.
This commit is contained in:
parent
f8f885966e
commit
e33962364d
@ -76,6 +76,8 @@ class InterfaceState
|
|||||||
{
|
{
|
||||||
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
|
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
|
||||||
friend class Interface; // allow Interface to set owner_ and priority_
|
friend class Interface; // allow Interface to set owner_ and priority_
|
||||||
|
friend class StagePrivate;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
enum Status
|
enum Status
|
||||||
{
|
{
|
||||||
|
|||||||
@ -153,9 +153,23 @@ void StagePrivate::setStatus(const InterfaceState* s, InterfaceState::Status sta
|
|||||||
if (s->priority().status() == status)
|
if (s->priority().status() == status)
|
||||||
return; // nothing changing
|
return; // nothing changing
|
||||||
|
|
||||||
// actually enable/disable the state
|
// if we should disable the state, only do so when there is no enabled alternative path
|
||||||
if (s->owner())
|
if (status == InterfaceState::DISABLED) {
|
||||||
|
auto solution_is_enabled = [](auto&& solution) {
|
||||||
|
return state<opposite<dir>()>(*solution)->priority().enabled();
|
||||||
|
};
|
||||||
|
const auto& alternatives = trajectories<opposite<dir>()>(*s);
|
||||||
|
auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled);
|
||||||
|
if (alternative_path != alternatives.cend())
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (s->owner()) {
|
||||||
|
// actually enable/disable the state
|
||||||
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
|
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
|
||||||
|
} else {
|
||||||
|
const_cast<InterfaceState*>(s)->priority_ = InterfaceState::Priority(s->priority(), status);
|
||||||
|
}
|
||||||
|
|
||||||
// To break symmetry between both ends of a partial solution sequence that gets disabled,
|
// To break symmetry between both ends of a partial solution sequence that gets disabled,
|
||||||
// we mark the first state with DISABLED_FAILED and all other states down the tree only with DISABLED.
|
// we mark the first state with DISABLED_FAILED and all other states down the tree only with DISABLED.
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user