mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
update state priorities in a container-specific fashion
Reorder interface list on priority updates. This requires the InterfaceState to store a pointer to the owning Interface.
This commit is contained in:
parent
facd6438c9
commit
5c43d700a8
@ -138,6 +138,8 @@ public:
|
|||||||
protected:
|
protected:
|
||||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||||
|
|
||||||
|
virtual void onNewSolution(const SolutionBase& s) override;
|
||||||
|
|
||||||
/// callback for new start states (received externally)
|
/// callback for new start states (received externally)
|
||||||
virtual void onNewStartState(Interface::iterator external, bool updated) = 0;
|
virtual void onNewStartState(Interface::iterator external, bool updated) = 0;
|
||||||
/// callback for new end states (received externally)
|
/// callback for new end states (received externally)
|
||||||
@ -192,6 +194,8 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
virtual void onNewSolution(const SolutionBase& s) = 0;
|
||||||
|
|
||||||
WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer());
|
WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer());
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -75,7 +75,7 @@ MOVEIT_CLASS_FORWARD(Introspection)
|
|||||||
*/
|
*/
|
||||||
class InterfaceState {
|
class InterfaceState {
|
||||||
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
|
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
|
||||||
friend class Interface; // Interface is allowed to change priority
|
friend class Interface; // allow Interface to set owner_ and priority_
|
||||||
public:
|
public:
|
||||||
/** InterfaceStates are ordered according to two values:
|
/** InterfaceStates are ordered according to two values:
|
||||||
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
|
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
|
||||||
@ -120,6 +120,7 @@ public:
|
|||||||
return this->priority_ < other.priority_;
|
return this->priority_ < other.priority_;
|
||||||
}
|
}
|
||||||
inline const Priority& priority() const { return priority_; }
|
inline const Priority& priority() const { return priority_; }
|
||||||
|
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()
|
||||||
@ -131,7 +132,10 @@ private:
|
|||||||
PropertyMap properties_;
|
PropertyMap properties_;
|
||||||
Solutions incoming_trajectories_;
|
Solutions incoming_trajectories_;
|
||||||
Solutions outgoing_trajectories_;
|
Solutions outgoing_trajectories_;
|
||||||
|
|
||||||
|
// members needed for priority scheduling in Interface list
|
||||||
Priority priority_;
|
Priority priority_;
|
||||||
|
Interface* owner_ = nullptr; // allow update of priority
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@ -141,12 +145,15 @@ public:
|
|||||||
typedef std::function<void(iterator it, bool updated)> NotifyFunction;
|
typedef std::function<void(iterator it, bool updated)> NotifyFunction;
|
||||||
Interface(const NotifyFunction ¬ify = NotifyFunction());
|
Interface(const NotifyFunction ¬ify = NotifyFunction());
|
||||||
|
|
||||||
// add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state
|
/// add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state
|
||||||
iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing);
|
iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing);
|
||||||
|
|
||||||
// clone an existing InterfaceState, but without its incoming/outgoing connections
|
/// clone an existing InterfaceState, but without its incoming/outgoing connections
|
||||||
iterator clone(const InterfaceState &state);
|
iterator clone(const InterfaceState &state);
|
||||||
|
|
||||||
|
/// update state's priority if new priority is smaller and call notify_
|
||||||
|
void updatePriority(InterfaceState &state, const InterfaceState::Priority &priority);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const NotifyFunction notify_;
|
const NotifyFunction notify_;
|
||||||
};
|
};
|
||||||
@ -164,13 +171,13 @@ public:
|
|||||||
inline const InterfaceState* end() const { return end_; }
|
inline const InterfaceState* end() const { return end_; }
|
||||||
|
|
||||||
inline void setStartState(const InterfaceState& state){
|
inline void setStartState(const InterfaceState& state){
|
||||||
assert(start_ == NULL);
|
assert(start_ == NULL); // only allow setting once (by Stage)
|
||||||
start_ = &state;
|
start_ = &state;
|
||||||
const_cast<InterfaceState&>(state).addOutgoing(this);
|
const_cast<InterfaceState&>(state).addOutgoing(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
inline void setEndState(const InterfaceState& state){
|
inline void setEndState(const InterfaceState& state){
|
||||||
assert(end_ == NULL);
|
assert(end_ == NULL); // only allow setting once (by Stage)
|
||||||
end_ = &state;
|
end_ = &state;
|
||||||
const_cast<InterfaceState&>(state).addIncoming(this);
|
const_cast<InterfaceState&>(state).addIncoming(this);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -235,25 +235,24 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
|||||||
|
|
||||||
SerialContainer::solution_container trace; trace.reserve(children.size());
|
SerialContainer::solution_container trace; trace.reserve(children.size());
|
||||||
|
|
||||||
// find all incoming trajectories connected to current solution
|
// find all incoming solution pathes ending at current solution
|
||||||
SolutionCollector incoming(num_before);
|
SolutionCollector incoming(num_before);
|
||||||
traverse<BACKWARD>(current, std::ref(incoming), trace);
|
traverse<BACKWARD>(current, std::ref(incoming), trace);
|
||||||
if (incoming.solutions.empty())
|
|
||||||
return; // no connection to front()
|
|
||||||
|
|
||||||
|
// find all outgoing solution pathes starting at current solution
|
||||||
// find all outgoing trajectories connected to current solution
|
|
||||||
SolutionCollector outgoing(num_after);
|
SolutionCollector outgoing(num_after);
|
||||||
traverse<FORWARD>(current, std::ref(outgoing), trace);
|
traverse<FORWARD>(current, std::ref(outgoing), trace);
|
||||||
if (outgoing.solutions.empty())
|
|
||||||
return; // no connection to back()
|
|
||||||
|
|
||||||
// collect (and sort) all solutions for all combinations of incoming + current + outgoing
|
// collect (and sort) all solutions spanning from start to end of this container
|
||||||
ordered<SerialSolution> sorted;
|
ordered<SerialSolution> sorted;
|
||||||
SerialContainer::solution_container solution;
|
SerialContainer::solution_container solution;
|
||||||
solution.reserve(children.size());
|
solution.reserve(children.size());
|
||||||
for (auto& in : incoming.solutions) {
|
for (auto& in : incoming.solutions) {
|
||||||
for (auto& out : outgoing.solutions) {
|
for (auto& out : outgoing.solutions) {
|
||||||
|
InterfaceState::Priority prio(in.first.size() + 1 + out.first.size(),
|
||||||
|
in.second + current.cost() + out.second);
|
||||||
|
// found a complete solution path connecting start to end?
|
||||||
|
if (prio.depth() == children.size()) {
|
||||||
assert(solution.empty());
|
assert(solution.empty());
|
||||||
// insert incoming solutions in reverse order
|
// insert incoming solutions in reverse order
|
||||||
solution.insert(solution.end(), in.first.rbegin(), in.first.rend());
|
solution.insert(solution.end(), in.first.rbegin(), in.first.rend());
|
||||||
@ -261,12 +260,19 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
|||||||
solution.push_back(¤t);
|
solution.push_back(¤t);
|
||||||
// insert outgoing solutions in normal order
|
// insert outgoing solutions in normal order
|
||||||
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
||||||
|
// store solution in sorted list
|
||||||
sorted.insert(SerialSolution(impl, std::move(solution), in.second + current.cost() + out.second));
|
sorted.insert(SerialSolution(impl, std::move(solution), prio.cost()));
|
||||||
|
} else {
|
||||||
|
// update state costs
|
||||||
|
const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start();
|
||||||
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// store new solutions
|
// store new solutions (in sorted)
|
||||||
for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it)
|
for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it)
|
||||||
impl->storeNewSolution(std::move(*it));
|
impl->storeNewSolution(std::move(*it));
|
||||||
}
|
}
|
||||||
@ -507,6 +513,18 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc
|
|||||||
throw errors;
|
throw errors;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void ParallelContainerBase::onNewSolution(const SolutionBase &s)
|
||||||
|
{
|
||||||
|
// update state priorities
|
||||||
|
InterfaceState::Priority prio(1, s.cost());
|
||||||
|
InterfaceState* start = const_cast<InterfaceState*>(s.start());
|
||||||
|
start->owner()->updatePriority(*start, prio);
|
||||||
|
InterfaceState* end = const_cast<InterfaceState*>(s.end());
|
||||||
|
end->owner()->updatePriority(*end, prio);
|
||||||
|
|
||||||
|
pimpl()->newSolution(s);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
|
WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
|
||||||
: ContainerBasePrivate(me, name)
|
: ContainerBasePrivate(me, name)
|
||||||
@ -569,6 +587,18 @@ Stage* WrapperBase::wrapped()
|
|||||||
return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get();
|
return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void WrapperBase::onNewSolution(const SolutionBase &s)
|
||||||
|
{
|
||||||
|
// update state priorities
|
||||||
|
InterfaceState::Priority prio(1, s.cost());
|
||||||
|
InterfaceState* start = const_cast<InterfaceState*>(s.start());
|
||||||
|
start->owner()->updatePriority(*start, prio);
|
||||||
|
InterfaceState* end = const_cast<InterfaceState*>(s.end());
|
||||||
|
end->owner()->updatePriority(*end, prio);
|
||||||
|
|
||||||
|
pimpl()->newSolution(s);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name)
|
WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name)
|
||||||
: WrapperBasePrivate(me, name)
|
: WrapperBasePrivate(me, name)
|
||||||
|
|||||||
@ -69,6 +69,9 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin
|
|||||||
// move state to a list node
|
// move state to a list node
|
||||||
std::list<InterfaceState> container;
|
std::list<InterfaceState> container;
|
||||||
auto it = container.insert(container.end(), std::move(state));
|
auto it = container.insert(container.end(), std::move(state));
|
||||||
|
assert(it->owner_ == nullptr);
|
||||||
|
it->owner_ = this;
|
||||||
|
|
||||||
// configure state: inherit priority from other end's state and add current solution's cost
|
// configure state: inherit priority from other end's state and add current solution's cost
|
||||||
assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set
|
assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set
|
||||||
if (incoming) {
|
if (incoming) {
|
||||||
@ -88,10 +91,20 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin
|
|||||||
Interface::iterator Interface::clone(const InterfaceState &state)
|
Interface::iterator Interface::clone(const InterfaceState &state)
|
||||||
{
|
{
|
||||||
iterator it = insert(InterfaceState(state));
|
iterator it = insert(InterfaceState(state));
|
||||||
|
it->owner_ = this;
|
||||||
if (notify_) notify_(it, false);
|
if (notify_) notify_(it, false);
|
||||||
return it;
|
return it;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Interface::updatePriority(InterfaceState &state, const InterfaceState::Priority& priority)
|
||||||
|
{
|
||||||
|
if (priority < state.priority()) {
|
||||||
|
auto it = std::find_if(begin(), end(), [&state](const InterfaceState& other) { return &state == &other; });
|
||||||
|
assert(it != end()); // state should be part of this interface
|
||||||
|
state.priority_ = priority;
|
||||||
|
if (notify_) notify_(it, true);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void SolutionBase::setCreator(StagePrivate *creator)
|
void SolutionBase::setCreator(StagePrivate *creator)
|
||||||
{
|
{
|
||||||
|
|||||||
@ -240,7 +240,7 @@ void Task::publishAllSolutions(bool wait)
|
|||||||
|
|
||||||
void Task::onNewSolution(const SolutionBase &s)
|
void Task::onNewSolution(const SolutionBase &s)
|
||||||
{
|
{
|
||||||
pimpl()->newSolution(s);
|
WrapperBase::onNewSolution(s);
|
||||||
if (introspection_)
|
if (introspection_)
|
||||||
introspection_->publishSolution(s);
|
introspection_->publishSolution(s);
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user