Rework updatePriority() functions

- Centrally distinguish between have owner() or not in InterfaceState::updatePriority()
- Have a separate updateStatus() method to just update the pruning status
- Split Interface::updatePriority() into a method taking the InterfaceState*
  and one taking an Interface::iterator (for efficiency)
- Early return in container.cpp's updateStatePrios()
This commit is contained in:
Robert Haschke 2021-11-19 14:37:22 +01:00
parent 4170a1c93a
commit 29d1e44c5d
4 changed files with 38 additions and 22 deletions

View File

@ -140,13 +140,21 @@ public:
/// states are ordered by priority
inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; }
inline const Priority& priority() const { return priority_; }
/// Update priority and call owner's notify() if possible
void updatePriority(const InterfaceState::Priority& priority);
/// Update status, but keep current priority
void updateStatus(Status status);
Interface* owner() const { return owner_; }
private:
// these methods should be only called by SolutionBase::set[Start|End]State()
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); }
// Set new priority without updating the owning interface (USE WITH CARE)
inline void setPriority(const Priority& prio) { priority_ = prio; }
private:
planning_scene::PlanningSceneConstPtr scene_;
@ -204,6 +212,8 @@ public:
/// update state's priority (and call notify_ if it really has changed)
void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority);
/// more efficient variant of the above, because we can skip searching the state
void updatePriority(Interface::iterator it, const InterfaceState::Priority& priority);
private:
const NotifyFunction notify_;

View File

@ -160,11 +160,7 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St
}
// actually enable/disable the state
if (s->owner()) {
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
} else {
const_cast<InterfaceState*>(s)->priority_ = InterfaceState::Priority(s->priority(), status);
}
const_cast<InterfaceState*>(s)->updateStatus(status);
// if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface
if (parent() && trajectories<dir>(*s).empty()) {
@ -434,18 +430,15 @@ struct SolutionCollector
SolutionSequence::container_type trace;
};
inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) {
if (state->owner()) // owner becomes NULL if state is removed from (pending) Interface list
state->owner()->updatePriority(const_cast<InterfaceState*>(state),
// update depth + cost, but keep current status
InterfaceState::Priority(prio, state->priority().status()));
}
// recursively update state priorities along solution path
template <Interface::Direction dir>
inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path,
const InterfaceState::Priority& prio) {
for (const SolutionBase* solution : partial_solution_path)
updateStatePrio(state<dir>(*solution), prio);
inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Priority& prio) {
InterfaceState::Priority priority(prio, s.priority().status());
if (s.priority() == priority)
return;
const_cast<InterfaceState&>(s).updatePriority(priority);
for (const SolutionBase* successor : trajectories<dir>(s))
updateStatePrios<dir>(*state<dir>(*successor), prio);
}
void SerialContainer::onNewSolution(const SolutionBase& current) {
@ -496,10 +489,8 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
}
if (prio.depth() > 1) {
// update state priorities along the whole partial solution path
updateStatePrio(current.start(), prio);
updateStatePrio(current.end(), prio);
updateStatePrios<Interface::BACKWARD>(in.first, prio);
updateStatePrios<Interface::FORWARD>(out.first, prio);
updateStatePrios<Interface::BACKWARD>(*current.start(), prio);
updateStatePrios<Interface::FORWARD>(*current.end(), prio);
}
}
}

View File

@ -722,7 +722,7 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
// but don't re-enable states that are marked DISABLED
// https://github.com/ros-planning/moveit_task_constructor/pull/221
if (oit->priority().status() == InterfaceState::Status::FAILED)
oit->owner()->updatePriority(&*oit,
oit->owner()->updatePriority(oit,
InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED));
pending.insert(make_pair<other>(it, oit));
}

View File

@ -82,6 +82,17 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other)
return cost() < other.cost();
}
void InterfaceState::updatePriority(const InterfaceState::Priority& priority) {
if (owner()) {
owner()->updatePriority(this, priority);
} else {
setPriority(priority);
}
}
void InterfaceState::updateStatus(Status status) {
updatePriority(InterfaceState::Priority(priority_, status));
}
Interface::Interface(const Interface::NotifyFunction& notify) : notify_(notify) {}
// Announce a new InterfaceState
@ -131,7 +142,11 @@ void Interface::updatePriority(InterfaceState* state, const InterfaceState::Prio
auto it = std::find(begin(), end(), state); // find iterator to state
assert(it != end()); // state should be part of this interface
state->priority_ = priority; // update priority
updatePriority(it, priority);
}
void Interface::updatePriority(Interface::iterator it, const InterfaceState::Priority& priority) {
it->priority_ = priority; // update priority
update(it); // update position in ordered list
if (notify_)
notify_(it, true); // notify callback