SerialContainer: fix priority propagation

- traverse all (also partial) solution paths
- and update priority at both ends
- remove pending state pairs if cost increased to infinity
This commit is contained in:
Robert Haschke 2018-02-14 09:04:26 +01:00
parent 469c6ccede
commit acffd39091
4 changed files with 23 additions and 14 deletions

View File

@ -104,6 +104,9 @@ public:
c.splice(at, other, pos);
return pos;
}
template<typename Predicate>
void remove_if(Predicate p) { c.remove_if(p); }
};
namespace detail {

View File

@ -232,9 +232,7 @@ struct SolutionCollector {
void operator()(const SerialContainer::solution_container& trace, double cost) {
// traced path should not extend past container boundaries
assert(trace.size() <= max_depth);
if (trace.size() == max_depth) // reached max depth
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;
@ -257,11 +255,11 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
SerialContainer::solution_container trace; trace.reserve(children.size());
// find all incoming solution pathes ending at current solution
// find all incoming solution paths ending at current solution
SolutionCollector incoming(num_before);
traverse<Interface::BACKWARD>(current, std::ref(incoming), trace);
// find all outgoing solution pathes starting at current solution
// find all outgoing solution paths starting at current solution
SolutionCollector outgoing(num_after);
traverse<Interface::FORWARD>(current, std::ref(outgoing), trace);
@ -284,7 +282,7 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
solution.insert(solution.end(), out.first.begin(), out.first.end());
// store solution in sorted list
sorted.insert(SerialSolution(impl, std::move(solution), prio.cost()));
} else {
} else if (prio.depth() > 1) {
// update state costs
const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start();
start->owner()->updatePriority(*const_cast<InterfaceState*>(start), prio);
@ -592,8 +590,10 @@ void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t)
auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t));
if (it->isFailure()) {
auto state_it = impl->states_.insert(impl->states_.end(), std::move(state));
// attach state (different for start / end) to trajectory
auto state_it = impl->states_.insert(impl->states_.end(), InterfaceState(state));
it->setStartState(*state_it);
state_it = impl->states_.insert(impl->states_.end(), std::move(state));
it->setEndState(*state_it);
impl->failures_.push_back(&*it);
} else {

View File

@ -88,10 +88,6 @@ void StagePrivate::newSolution(const SolutionBase &solution)
if (introspection_)
introspection_->registerSolution(solution);
// ignore invalid / failure solutions
if (solution.isFailure())
return;
// call solution callbacks for both, valid solutions and failures
for (const auto& cb : solution_cbs_)
cb(solution);
@ -486,8 +482,13 @@ ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
void ConnectingPrivate::newStartState(Interface::iterator it, bool updated)
{
if (!std::isfinite(it->priority().cost()))
// TODO: only consider interface states with priority depth > threshold
if (!std::isfinite(it->priority().cost())) {
// remove pending pairs, if cost updated to infinity
if (updated)
pending.remove_if([it](const StatePair& p) { return p.first == it; });
return;
}
if (updated) {
// many pairs might be affected: sort
pending.sort();
@ -502,8 +503,12 @@ void ConnectingPrivate::newStartState(Interface::iterator it, bool updated)
void ConnectingPrivate::newEndState(Interface::iterator it, bool updated)
{
if (!std::isfinite(it->priority().cost()))
if (!std::isfinite(it->priority().cost())) {
// remove pending pairs, if cost updated to infinity
if (updated)
pending.remove_if([it](const StatePair& p) { return p.second == it; });
return;
}
if (updated) {
// many pairs might be affected: sort
pending.sort();

View File

@ -100,7 +100,8 @@ void Interface::updatePriority(InterfaceState &state, const InterfaceState::Prio
{
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 should be part of the interface
assert(it != end());
state.priority_ = priority;
if (notify_) notify_(it, true);
}