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
|
/// states are ordered by priority
|
||||||
inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; }
|
inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; }
|
||||||
|
|
||||||
inline const Priority& priority() const { return 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_; }
|
Interface* owner() const { return owner_; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// these methods should be only called by SolutionBase::set[Start|End]State()
|
// these methods should be only called by SolutionBase::set[Start|End]State()
|
||||||
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
|
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
|
||||||
inline void addOutgoing(SolutionBase* t) { outgoing_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:
|
private:
|
||||||
planning_scene::PlanningSceneConstPtr scene_;
|
planning_scene::PlanningSceneConstPtr scene_;
|
||||||
@ -204,6 +212,8 @@ public:
|
|||||||
|
|
||||||
/// update state's priority (and call notify_ if it really has changed)
|
/// update state's priority (and call notify_ if it really has changed)
|
||||||
void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority);
|
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:
|
private:
|
||||||
const NotifyFunction notify_;
|
const NotifyFunction notify_;
|
||||||
|
|||||||
@ -160,11 +160,7 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St
|
|||||||
}
|
}
|
||||||
|
|
||||||
// actually enable/disable the state
|
// actually enable/disable the state
|
||||||
if (s->owner()) {
|
const_cast<InterfaceState*>(s)->updateStatus(status);
|
||||||
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
|
|
||||||
} else {
|
|
||||||
const_cast<InterfaceState*>(s)->priority_ = InterfaceState::Priority(s->priority(), status);
|
|
||||||
}
|
|
||||||
|
|
||||||
// if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface
|
// if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface
|
||||||
if (parent() && trajectories<dir>(*s).empty()) {
|
if (parent() && trajectories<dir>(*s).empty()) {
|
||||||
@ -434,18 +430,15 @@ struct SolutionCollector
|
|||||||
SolutionSequence::container_type trace;
|
SolutionSequence::container_type trace;
|
||||||
};
|
};
|
||||||
|
|
||||||
inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) {
|
// recursively update state priorities along solution path
|
||||||
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()));
|
|
||||||
}
|
|
||||||
|
|
||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path,
|
inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Priority& prio) {
|
||||||
const InterfaceState::Priority& prio) {
|
InterfaceState::Priority priority(prio, s.priority().status());
|
||||||
for (const SolutionBase* solution : partial_solution_path)
|
if (s.priority() == priority)
|
||||||
updateStatePrio(state<dir>(*solution), prio);
|
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) {
|
void SerialContainer::onNewSolution(const SolutionBase& current) {
|
||||||
@ -496,10 +489,8 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
|
|||||||
}
|
}
|
||||||
if (prio.depth() > 1) {
|
if (prio.depth() > 1) {
|
||||||
// update state priorities along the whole partial solution path
|
// update state priorities along the whole partial solution path
|
||||||
updateStatePrio(current.start(), prio);
|
updateStatePrios<Interface::BACKWARD>(*current.start(), prio);
|
||||||
updateStatePrio(current.end(), prio);
|
updateStatePrios<Interface::FORWARD>(*current.end(), prio);
|
||||||
updateStatePrios<Interface::BACKWARD>(in.first, prio);
|
|
||||||
updateStatePrios<Interface::FORWARD>(out.first, prio);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -722,7 +722,7 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
|||||||
// but don't re-enable states that are marked DISABLED
|
// but don't re-enable states that are marked DISABLED
|
||||||
// https://github.com/ros-planning/moveit_task_constructor/pull/221
|
// https://github.com/ros-planning/moveit_task_constructor/pull/221
|
||||||
if (oit->priority().status() == InterfaceState::Status::FAILED)
|
if (oit->priority().status() == InterfaceState::Status::FAILED)
|
||||||
oit->owner()->updatePriority(&*oit,
|
oit->owner()->updatePriority(oit,
|
||||||
InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED));
|
InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED));
|
||||||
pending.insert(make_pair<other>(it, oit));
|
pending.insert(make_pair<other>(it, oit));
|
||||||
}
|
}
|
||||||
|
|||||||
@ -82,6 +82,17 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other)
|
|||||||
return cost() < other.cost();
|
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) {}
|
Interface::Interface(const Interface::NotifyFunction& notify) : notify_(notify) {}
|
||||||
|
|
||||||
// Announce a new InterfaceState
|
// 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
|
auto it = std::find(begin(), end(), state); // find iterator to state
|
||||||
assert(it != end()); // state should be part of this interface
|
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
|
update(it); // update position in ordered list
|
||||||
if (notify_)
|
if (notify_)
|
||||||
notify_(it, true); // notify callback
|
notify_(it, true); // notify callback
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user