mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
4170a1c93a
commit
29d1e44c5d
@ -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_;
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user