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);
|
c.splice(at, other, pos);
|
||||||
return pos;
|
return pos;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
template<typename Predicate>
|
||||||
|
void remove_if(Predicate p) { c.remove_if(p); }
|
||||||
};
|
};
|
||||||
|
|
||||||
namespace detail {
|
namespace detail {
|
||||||
|
|||||||
@ -232,9 +232,7 @@ struct SolutionCollector {
|
|||||||
void operator()(const SerialContainer::solution_container& trace, double cost) {
|
void operator()(const SerialContainer::solution_container& trace, double cost) {
|
||||||
// traced path should not extend past container boundaries
|
// traced path should not extend past container boundaries
|
||||||
assert(trace.size() <= max_depth);
|
assert(trace.size() <= max_depth);
|
||||||
|
solutions.emplace_back(std::make_pair(trace, cost));
|
||||||
if (trace.size() == max_depth) // reached max depth
|
|
||||||
solutions.emplace_back(std::make_pair(trace, cost));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
std::list<std::pair<SerialContainer::solution_container, double>> solutions;
|
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());
|
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);
|
SolutionCollector incoming(num_before);
|
||||||
traverse<Interface::BACKWARD>(current, std::ref(incoming), trace);
|
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);
|
SolutionCollector outgoing(num_after);
|
||||||
traverse<Interface::FORWARD>(current, std::ref(outgoing), trace);
|
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());
|
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
||||||
// 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 {
|
} else if (prio.depth() > 1) {
|
||||||
// update state costs
|
// update state costs
|
||||||
const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start();
|
const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start();
|
||||||
start->owner()->updatePriority(*const_cast<InterfaceState*>(start), prio);
|
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));
|
auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t));
|
||||||
|
|
||||||
if (it->isFailure()) {
|
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);
|
it->setStartState(*state_it);
|
||||||
|
state_it = impl->states_.insert(impl->states_.end(), std::move(state));
|
||||||
it->setEndState(*state_it);
|
it->setEndState(*state_it);
|
||||||
impl->failures_.push_back(&*it);
|
impl->failures_.push_back(&*it);
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@ -88,10 +88,6 @@ void StagePrivate::newSolution(const SolutionBase &solution)
|
|||||||
if (introspection_)
|
if (introspection_)
|
||||||
introspection_->registerSolution(solution);
|
introspection_->registerSolution(solution);
|
||||||
|
|
||||||
// ignore invalid / failure solutions
|
|
||||||
if (solution.isFailure())
|
|
||||||
return;
|
|
||||||
|
|
||||||
// call solution callbacks for both, valid solutions and failures
|
// call solution callbacks for both, valid solutions and failures
|
||||||
for (const auto& cb : solution_cbs_)
|
for (const auto& cb : solution_cbs_)
|
||||||
cb(solution);
|
cb(solution);
|
||||||
@ -486,8 +482,13 @@ ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
|
|||||||
|
|
||||||
void ConnectingPrivate::newStartState(Interface::iterator it, bool updated)
|
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;
|
return;
|
||||||
|
}
|
||||||
if (updated) {
|
if (updated) {
|
||||||
// many pairs might be affected: sort
|
// many pairs might be affected: sort
|
||||||
pending.sort();
|
pending.sort();
|
||||||
@ -502,8 +503,12 @@ void ConnectingPrivate::newStartState(Interface::iterator it, bool updated)
|
|||||||
|
|
||||||
void ConnectingPrivate::newEndState(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;
|
return;
|
||||||
|
}
|
||||||
if (updated) {
|
if (updated) {
|
||||||
// many pairs might be affected: sort
|
// many pairs might be affected: sort
|
||||||
pending.sort();
|
pending.sort();
|
||||||
|
|||||||
@ -100,7 +100,8 @@ void Interface::updatePriority(InterfaceState &state, const InterfaceState::Prio
|
|||||||
{
|
{
|
||||||
if (priority < state.priority()) {
|
if (priority < state.priority()) {
|
||||||
auto it = std::find_if(begin(), end(), [&state](const InterfaceState& other) { return &state == &other; });
|
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;
|
state.priority_ = priority;
|
||||||
if (notify_) notify_(it, true);
|
if (notify_) notify_(it, true);
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user