From 29d1e44c5da4649ee5a31a2903dfc3a57c27e341 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Nov 2021 14:37:22 +0100 Subject: [PATCH] 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() --- .../include/moveit/task_constructor/storage.h | 10 ++++++ core/src/container.cpp | 31 +++++++------------ core/src/stage.cpp | 2 +- core/src/storage.cpp | 17 +++++++++- 4 files changed, 38 insertions(+), 22 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 26ac05f7..22322a97 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -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_; diff --git a/core/src/container.cpp b/core/src/container.cpp index 2449e21e..dea2c4b0 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -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(s), InterfaceState::Priority(s->priority(), status)); - } else { - const_cast(s)->priority_ = InterfaceState::Priority(s->priority(), status); - } + const_cast(s)->updateStatus(status); // if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface if (parent() && trajectories(*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(state), - // update depth + cost, but keep current status - InterfaceState::Priority(prio, state->priority().status())); -} - +// recursively update state priorities along solution path template -inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path, - const InterfaceState::Priority& prio) { - for (const SolutionBase* solution : partial_solution_path) - updateStatePrio(state(*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(s).updatePriority(priority); + for (const SolutionBase* successor : trajectories(s)) + updateStatePrios(*state(*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(in.first, prio); - updateStatePrios(out.first, prio); + updateStatePrios(*current.start(), prio); + updateStatePrios(*current.end(), prio); } } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 4140922a..841c4532 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -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(it, oit)); } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 08185225..f88ac960 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -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