mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
reset InterfaceState::owner_ if state is removed from Interface
This commit is contained in:
parent
1f4264c8c8
commit
1eacea6b35
@ -152,11 +152,22 @@ public:
|
||||
/// clone an existing InterfaceState, but without its incoming/outgoing connections
|
||||
iterator clone(const InterfaceState &state);
|
||||
|
||||
/// remove a state from the interface and return it as a one-element list
|
||||
container_type remove(iterator it);
|
||||
|
||||
/// update state's priority if new priority is smaller and call notify_
|
||||
void updatePriority(InterfaceState &state, const InterfaceState::Priority &priority);
|
||||
|
||||
private:
|
||||
const NotifyFunction notify_;
|
||||
|
||||
// restrict access to some functions to ensure consistency
|
||||
// (we need to set/unset InterfaceState::owner_)
|
||||
using ordered<InterfaceState>::moveTo;
|
||||
using ordered<InterfaceState>::moveFrom;
|
||||
using ordered<InterfaceState>::insert;
|
||||
using ordered<InterfaceState>::erase;
|
||||
using ordered<InterfaceState>::remove_if;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -283,11 +283,11 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
||||
// store solution in sorted list
|
||||
sorted.insert(SerialSolution(impl, std::move(solution), prio.cost()));
|
||||
} else if (prio.depth() > 1) {
|
||||
// update state costs
|
||||
// update state priority at both ends of the partial solution path
|
||||
const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start();
|
||||
start->owner()->updatePriority(*const_cast<InterfaceState*>(start), prio);
|
||||
if (start->owner()) start->owner()->updatePriority(*const_cast<InterfaceState*>(start), prio);
|
||||
const InterfaceState* end = (out.first.empty() ? current : *out.first.back()).end();
|
||||
end->owner()->updatePriority(*const_cast<InterfaceState*>(end), prio);
|
||||
if (end->owner()) end->owner()->updatePriority(*const_cast<InterfaceState*>(end), prio);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -262,12 +262,12 @@ PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *m
|
||||
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
|
||||
// move infinite-cost states to processed list
|
||||
if (std::isinf(state->priority().cost()))
|
||||
starts_->moveTo(state, processed, processed.end());
|
||||
processed.splice(processed.end(), starts_->remove(state));
|
||||
}
|
||||
void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) {
|
||||
// move infinite-cost states to processed list
|
||||
if (std::isinf(state->priority().cost()))
|
||||
ends_->moveTo(state, processed, processed.end());
|
||||
processed.splice(processed.end(), ends_->remove(state));
|
||||
}
|
||||
|
||||
inline bool PropagatingEitherWayPrivate::hasStartState() const{
|
||||
@ -276,8 +276,9 @@ inline bool PropagatingEitherWayPrivate::hasStartState() const{
|
||||
|
||||
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
|
||||
assert(hasStartState());
|
||||
// move state to processed list
|
||||
return *starts_->moveTo(starts_->begin(), processed, processed.end());
|
||||
// move state to end of processed list
|
||||
processed.splice(processed.end(), starts_->remove(starts_->begin()));
|
||||
return processed.back();
|
||||
}
|
||||
|
||||
inline bool PropagatingEitherWayPrivate::hasEndState() const{
|
||||
@ -287,7 +288,8 @@ inline bool PropagatingEitherWayPrivate::hasEndState() const{
|
||||
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
|
||||
assert(hasEndState());
|
||||
// move state to processed list
|
||||
return *ends_->moveTo(ends_->begin(), processed, processed.end());
|
||||
processed.splice(processed.end(), ends_->remove(ends_->begin()));
|
||||
return processed.back();
|
||||
}
|
||||
|
||||
bool PropagatingEitherWayPrivate::canCompute() const
|
||||
|
||||
@ -96,6 +96,14 @@ Interface::iterator Interface::clone(const InterfaceState &state)
|
||||
return it;
|
||||
}
|
||||
|
||||
Interface::container_type Interface::remove(iterator it)
|
||||
{
|
||||
container_type result;
|
||||
moveTo(it, result, result.end());
|
||||
it->owner_ = nullptr;
|
||||
return result;
|
||||
}
|
||||
|
||||
void Interface::updatePriority(InterfaceState &state, const InterfaceState::Priority& priority)
|
||||
{
|
||||
if (priority < state.priority()) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user