mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
DISABLED -> PRUNED
This makes the semantics much clearer as states can only be disabled by pruning.
This commit is contained in:
parent
d143dd7076
commit
35560d897d
@ -148,7 +148,7 @@ protected:
|
|||||||
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Set ENABLED / DISABLED status of the solution tree starting from s into given direction
|
/// Set ENABLED / PRUNED status of the solution tree starting from s into given direction
|
||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
void setStatus(const InterfaceState* s, InterfaceState::Status status);
|
void setStatus(const InterfaceState* s, InterfaceState::Status status);
|
||||||
|
|
||||||
|
|||||||
@ -82,7 +82,7 @@ public:
|
|||||||
enum Status
|
enum Status
|
||||||
{
|
{
|
||||||
ENABLED, // state is actively considered during planning
|
ENABLED, // state is actively considered during planning
|
||||||
DISABLED, // state is disabled because a required connected state failed
|
PRUNED, // state is disabled because a required connected state failed
|
||||||
FAILED, // state that failed, causing the whole partial solution to be disabled
|
FAILED, // state that failed, causing the whole partial solution to be disabled
|
||||||
};
|
};
|
||||||
/** InterfaceStates are ordered according to two values:
|
/** InterfaceStates are ordered according to two values:
|
||||||
@ -101,6 +101,7 @@ public:
|
|||||||
inline Status status() const { return std::get<0>(*this); }
|
inline Status status() const { return std::get<0>(*this); }
|
||||||
inline bool enabled() const { return std::get<0>(*this) == ENABLED; }
|
inline bool enabled() const { return std::get<0>(*this) == ENABLED; }
|
||||||
inline bool failed() const { return std::get<0>(*this) == FAILED; }
|
inline bool failed() const { return std::get<0>(*this) == FAILED; }
|
||||||
|
inline bool pruned() const { return std::get<0>(*this) == PRUNED; }
|
||||||
|
|
||||||
inline unsigned int depth() const { return std::get<1>(*this); }
|
inline unsigned int depth() const { return std::get<1>(*this); }
|
||||||
inline double cost() const { return std::get<2>(*this); }
|
inline double cost() const { return std::get<2>(*this); }
|
||||||
|
|||||||
@ -144,7 +144,7 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St
|
|||||||
return; // nothing changing
|
return; // nothing changing
|
||||||
|
|
||||||
// if we should disable the state, only do so when there is no enabled alternative path
|
// if we should disable the state, only do so when there is no enabled alternative path
|
||||||
if (status == InterfaceState::DISABLED) {
|
if (status == InterfaceState::PRUNED) {
|
||||||
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();
|
||||||
};
|
};
|
||||||
@ -177,13 +177,13 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St
|
|||||||
}
|
}
|
||||||
|
|
||||||
// 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 FAILED and all other states down the tree only with DISABLED.
|
// we mark the first state with FAILED and all other states down the tree only with PRUNED.
|
||||||
// This allows us to re-enable the FAILED side, while not (yet) consider the DISABLED states again,
|
// This allows us to re-enable the FAILED side, while not (yet) consider the PRUNED states again,
|
||||||
// when new states arrive in a Connecting stage.
|
// when new states arrive in a Connecting stage.
|
||||||
// All DISABLED states are only re-enabled if the FAILED state actually gets connected.
|
// All PRUNED states are only re-enabled if the FAILED state actually gets connected.
|
||||||
// For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221
|
// For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221
|
||||||
if (status == InterfaceState::Status::FAILED)
|
if (status == InterfaceState::Status::FAILED)
|
||||||
status = InterfaceState::Status::DISABLED; // 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>(*s))
|
||||||
|
|||||||
@ -712,7 +712,7 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
|
|||||||
template <Interface::Direction other>
|
template <Interface::Direction other>
|
||||||
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
|
||||||
if (it->priority().status() == InterfaceState::DISABLED)
|
if (it->priority().pruned())
|
||||||
// remove all pending pairs involving this state
|
// remove all pending pairs involving this state
|
||||||
pending.remove_if([it](const StatePair& p) { return std::get<opposite<other>()>(p) == it; });
|
pending.remove_if([it](const StatePair& p) { return std::get<opposite<other>()>(p) == it; });
|
||||||
else
|
else
|
||||||
|
|||||||
@ -148,7 +148,7 @@ std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio)
|
|||||||
// maps InterfaceState::Status values to output (color-changing) prefix
|
// maps InterfaceState::Status values to output (color-changing) prefix
|
||||||
static const char* prefix[] = {
|
static const char* prefix[] = {
|
||||||
"\033[32me:", // ENABLED - green
|
"\033[32me:", // ENABLED - green
|
||||||
"\033[33md:", // DISABLED - yellow
|
"\033[33md:", // PRUNED - yellow
|
||||||
"\033[31mf:", // FAILED - red
|
"\033[31mf:", // FAILED - red
|
||||||
};
|
};
|
||||||
static const char* color_reset = "\033[m";
|
static const char* color_reset = "\033[m";
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user