mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'fix-containers' and 'fix-priority-updates'
This commit is contained in:
commit
cf76352d2d
@ -152,11 +152,22 @@ public:
|
|||||||
/// 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);
|
||||||
|
|
||||||
|
/// remove a state from the interface and return it as a one-element list
|
||||||
|
container_type remove(iterator it);
|
||||||
|
|
||||||
/// update state's priority if new priority is smaller and call notify_
|
/// update state's priority if new priority is smaller and call notify_
|
||||||
void updatePriority(InterfaceState &state, const InterfaceState::Priority &priority);
|
void updatePriority(InterfaceState *state, const InterfaceState::Priority &priority);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const NotifyFunction notify_;
|
const NotifyFunction notify_;
|
||||||
|
|
||||||
|
// restrict access to some functions to ensure consistency
|
||||||
|
// (we need to set/unset InterfaceState::owner_)
|
||||||
|
using ordered<InterfaceState>::moveTo;
|
||||||
|
using ordered<InterfaceState>::moveFrom;
|
||||||
|
using ordered<InterfaceState>::insert;
|
||||||
|
using ordered<InterfaceState>::erase;
|
||||||
|
using ordered<InterfaceState>::remove_if;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -256,10 +256,25 @@ struct SolutionCollector {
|
|||||||
solutions.emplace_back(std::make_pair(trace, cost));
|
solutions.emplace_back(std::make_pair(trace, cost));
|
||||||
}
|
}
|
||||||
|
|
||||||
std::list<std::pair<SerialContainer::solution_container, double>> solutions;
|
typedef std::list<std::pair<SerialContainer::solution_container, double>> SolutionCostPairs;
|
||||||
|
SolutionCostPairs solutions;
|
||||||
const size_t max_depth;
|
const size_t max_depth;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
void updateStateCosts(const SerialContainer::solution_container &partial_solution_path,
|
||||||
|
const InterfaceState::Priority &prio) {
|
||||||
|
for (const SolutionBase* solution : partial_solution_path) {
|
||||||
|
// here it suffices to update the start state, because the end state is the start state
|
||||||
|
// of the next solution (they are all connected)
|
||||||
|
InterfaceState* state = const_cast<InterfaceState*>(solution->start());
|
||||||
|
if (state->owner()) state->owner()->updatePriority(state, prio);
|
||||||
|
}
|
||||||
|
// finally update the end state of the last solution
|
||||||
|
if (partial_solution_path.empty()) return;
|
||||||
|
InterfaceState* state = const_cast<InterfaceState*>(partial_solution_path.back()->end());
|
||||||
|
if (state->owner()) state->owner()->updatePriority(state, prio);
|
||||||
|
}
|
||||||
|
|
||||||
void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
||||||
{
|
{
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
@ -304,11 +319,10 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
|||||||
// store solution in sorted list
|
// store solution in sorted list
|
||||||
sorted.insert(SerialSolution(impl, std::move(solution), prio.cost()));
|
sorted.insert(SerialSolution(impl, std::move(solution), prio.cost()));
|
||||||
} else if (prio.depth() > 1) {
|
} else if (prio.depth() > 1) {
|
||||||
// update state costs
|
// update state priorities along the whole partial solution path
|
||||||
const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start();
|
updateStateCosts(in.first, prio);
|
||||||
start->owner()->updatePriority(*const_cast<InterfaceState*>(start), prio);
|
updateStateCosts({¤t}, prio);
|
||||||
const InterfaceState* end = (out.first.empty() ? current : *out.first.back()).end();
|
updateStateCosts(out.first, prio);
|
||||||
end->owner()->updatePriority(*const_cast<InterfaceState*>(end), prio);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -306,12 +306,12 @@ InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
|
|||||||
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
|
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
|
||||||
// move infinite-cost states to processed list
|
// move infinite-cost states to processed list
|
||||||
if (std::isinf(state->priority().cost()))
|
if (std::isinf(state->priority().cost()))
|
||||||
starts_->moveTo(state, processed, processed.end());
|
processed.splice(processed.end(), starts_->remove(state));
|
||||||
}
|
}
|
||||||
void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) {
|
void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) {
|
||||||
// move infinite-cost states to processed list
|
// move infinite-cost states to processed list
|
||||||
if (std::isinf(state->priority().cost()))
|
if (std::isinf(state->priority().cost()))
|
||||||
ends_->moveTo(state, processed, processed.end());
|
processed.splice(processed.end(), ends_->remove(state));
|
||||||
}
|
}
|
||||||
|
|
||||||
inline bool PropagatingEitherWayPrivate::hasStartState() const{
|
inline bool PropagatingEitherWayPrivate::hasStartState() const{
|
||||||
@ -320,8 +320,9 @@ inline bool PropagatingEitherWayPrivate::hasStartState() const{
|
|||||||
|
|
||||||
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
|
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
|
||||||
assert(hasStartState());
|
assert(hasStartState());
|
||||||
// move state to processed list
|
// move state to end of processed list
|
||||||
return *starts_->moveTo(starts_->begin(), processed, processed.end());
|
processed.splice(processed.end(), starts_->remove(starts_->begin()));
|
||||||
|
return processed.back();
|
||||||
}
|
}
|
||||||
|
|
||||||
inline bool PropagatingEitherWayPrivate::hasEndState() const{
|
inline bool PropagatingEitherWayPrivate::hasEndState() const{
|
||||||
@ -331,7 +332,8 @@ inline bool PropagatingEitherWayPrivate::hasEndState() const{
|
|||||||
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
|
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
|
||||||
assert(hasEndState());
|
assert(hasEndState());
|
||||||
// move state to processed list
|
// move state to processed list
|
||||||
return *ends_->moveTo(ends_->begin(), processed, processed.end());
|
processed.splice(processed.end(), ends_->remove(ends_->begin()));
|
||||||
|
return processed.back();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PropagatingEitherWayPrivate::canCompute() const
|
bool PropagatingEitherWayPrivate::canCompute() const
|
||||||
|
|||||||
@ -96,13 +96,21 @@ Interface::iterator Interface::clone(const InterfaceState &state)
|
|||||||
return it;
|
return it;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Interface::updatePriority(InterfaceState &state, const InterfaceState::Priority& priority)
|
Interface::container_type Interface::remove(iterator it)
|
||||||
{
|
{
|
||||||
if (priority < state.priority()) {
|
container_type result;
|
||||||
auto it = std::find_if(begin(), end(), [&state](const InterfaceState& other) { return &state == &other; });
|
moveTo(it, result, result.end());
|
||||||
|
it->owner_ = nullptr;
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
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; });
|
||||||
// state should be part of the interface
|
// state should be part of the interface
|
||||||
assert(it != end());
|
assert(it != end());
|
||||||
state.priority_ = priority;
|
state->priority_ = priority;
|
||||||
if (notify_) notify_(it, true);
|
if (notify_) notify_(it, true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user