mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
469c6ccede
commit
acffd39091
@ -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 {
|
||||
|
||||
@ -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 ¤t)
|
||||
|
||||
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 ¤t)
|
||||
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 {
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user