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:
|
||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||
|
||||
virtual void onNewSolution(const SolutionBase& s) override;
|
||||
|
||||
/// callback for new start states (received externally)
|
||||
virtual void onNewStartState(Interface::iterator external, bool updated) = 0;
|
||||
/// callback for new end states (received externally)
|
||||
@ -192,6 +194,8 @@ public:
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void onNewSolution(const SolutionBase& s) = 0;
|
||||
|
||||
WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer());
|
||||
};
|
||||
|
||||
|
||||
@ -75,7 +75,7 @@ MOVEIT_CLASS_FORWARD(Introspection)
|
||||
*/
|
||||
class InterfaceState {
|
||||
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:
|
||||
/** InterfaceStates are ordered according to two values:
|
||||
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
|
||||
@ -120,6 +120,7 @@ public:
|
||||
return this->priority_ < other.priority_;
|
||||
}
|
||||
inline const Priority& priority() const { return priority_; }
|
||||
Interface* owner() const { return owner_; }
|
||||
|
||||
private:
|
||||
// these methods should be only called by SolutionBase::set[Start|End]State()
|
||||
@ -131,7 +132,10 @@ private:
|
||||
PropertyMap properties_;
|
||||
Solutions incoming_trajectories_;
|
||||
Solutions outgoing_trajectories_;
|
||||
|
||||
// members needed for priority scheduling in Interface list
|
||||
Priority priority_;
|
||||
Interface* owner_ = nullptr; // allow update of priority
|
||||
};
|
||||
|
||||
|
||||
@ -141,12 +145,15 @@ public:
|
||||
typedef std::function<void(iterator it, bool updated)> 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);
|
||||
|
||||
// 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);
|
||||
|
||||
/// update state's priority if new priority is smaller and call notify_
|
||||
void updatePriority(InterfaceState &state, const InterfaceState::Priority &priority);
|
||||
|
||||
private:
|
||||
const NotifyFunction notify_;
|
||||
};
|
||||
@ -164,13 +171,13 @@ public:
|
||||
inline const InterfaceState* end() const { return end_; }
|
||||
|
||||
inline void setStartState(const InterfaceState& state){
|
||||
assert(start_ == NULL);
|
||||
assert(start_ == NULL); // only allow setting once (by Stage)
|
||||
start_ = &state;
|
||||
const_cast<InterfaceState&>(state).addOutgoing(this);
|
||||
}
|
||||
|
||||
inline void setEndState(const InterfaceState& state){
|
||||
assert(end_ == NULL);
|
||||
assert(end_ == NULL); // only allow setting once (by Stage)
|
||||
end_ = &state;
|
||||
const_cast<InterfaceState&>(state).addIncoming(this);
|
||||
}
|
||||
|
||||
@ -235,25 +235,24 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
||||
|
||||
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);
|
||||
traverse<BACKWARD>(current, std::ref(incoming), trace);
|
||||
if (incoming.solutions.empty())
|
||||
return; // no connection to front()
|
||||
|
||||
|
||||
// find all outgoing trajectories connected to current solution
|
||||
// find all outgoing solution pathes starting at current solution
|
||||
SolutionCollector outgoing(num_after);
|
||||
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;
|
||||
SerialContainer::solution_container solution;
|
||||
solution.reserve(children.size());
|
||||
for (auto& in : incoming.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());
|
||||
// insert incoming solutions in reverse order
|
||||
solution.insert(solution.end(), in.first.rbegin(), in.first.rend());
|
||||
@ -261,12 +260,19 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
||||
solution.push_back(¤t);
|
||||
// insert outgoing solutions in normal order
|
||||
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
||||
|
||||
sorted.insert(SerialSolution(impl, std::move(solution), in.second + current.cost() + out.second));
|
||||
// store solution in sorted list
|
||||
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)
|
||||
impl->storeNewSolution(std::move(*it));
|
||||
}
|
||||
@ -507,6 +513,18 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc
|
||||
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)
|
||||
: ContainerBasePrivate(me, name)
|
||||
@ -569,6 +587,18 @@ Stage* WrapperBase::wrapped()
|
||||
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)
|
||||
: WrapperBasePrivate(me, name)
|
||||
|
||||
@ -69,6 +69,9 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin
|
||||
// move state to a list node
|
||||
std::list<InterfaceState> container;
|
||||
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
|
||||
assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set
|
||||
if (incoming) {
|
||||
@ -88,10 +91,20 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin
|
||||
Interface::iterator Interface::clone(const InterfaceState &state)
|
||||
{
|
||||
iterator it = insert(InterfaceState(state));
|
||||
it->owner_ = this;
|
||||
if (notify_) notify_(it, false);
|
||||
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)
|
||||
{
|
||||
|
||||
@ -240,7 +240,7 @@ void Task::publishAllSolutions(bool wait)
|
||||
|
||||
void Task::onNewSolution(const SolutionBase &s)
|
||||
{
|
||||
pimpl()->newSolution(s);
|
||||
WrapperBase::onNewSolution(s);
|
||||
if (introspection_)
|
||||
introspection_->publishSolution(s);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user