Simplify SolutionCollector

This commit is contained in:
Robert Haschke 2020-12-05 13:46:27 +01:00
parent 576a1aa48d
commit c8e75ab205
3 changed files with 46 additions and 51 deletions

View File

@ -98,17 +98,6 @@ protected:
/// called by a (direct) child when a new solution becomes available
void onNewSolution(const SolutionBase& s) override;
using SolutionProcessor =
std::function<void(const SolutionSequence::container_type&, const InterfaceState::Priority&)>;
/// Traverse all solution pathes starting at start and going in given direction dir
/// until the end, i.e. until there are no more subsolutions in the given direction
/// For each solution path, callback the given processor passing
/// the full trace (from start to end, but not including start) and its accumulated costs
template <Interface::Direction dir>
void traverse(const SolutionBase& start, const SolutionProcessor& cb, SolutionSequence::container_type& trace,
const InterfaceState::Priority& = InterfaceState::Priority(0, 0.0));
protected:
SerialContainer(SerialContainerPrivate* impl);
};

View File

@ -207,10 +207,6 @@ public:
inline const InterfaceState* start() const { return start_; }
inline const InterfaceState* end() const { return end_; }
/// Retrieve following (FORWARD) or preceding (BACKWARD) solution segments
template <Interface::Direction dir>
inline const InterfaceState::Solutions& trajectories() const;
/** set the solution's start_state_
*
* Must not be used with different states because it registers the solution with the state as well.
@ -367,13 +363,28 @@ private:
const SolutionBase* wrapped_;
};
/// Trait to retrieve the end (FORWARD) or start (BACKWARD) state of a given solution
template <Interface::Direction dir>
const InterfaceState* state(const SolutionBase& solution);
template <>
inline const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const {
return end_->outgoingTrajectories();
inline const InterfaceState* state<Interface::FORWARD>(const SolutionBase& solution) {
return solution.end();
}
template <>
inline const InterfaceState::Solutions& SolutionBase::trajectories<Interface::BACKWARD>() const {
return start_->incomingTrajectories();
inline const InterfaceState* state<Interface::BACKWARD>(const SolutionBase& solution) {
return solution.start();
}
/// Trait to retrieve outgoing (FORWARD) or incoming (BACKWARD) solution segments of a given state
template <Interface::Direction dir>
const InterfaceState::Solutions& trajectories(const InterfaceState* state);
template <>
inline const InterfaceState::Solutions& trajectories<Interface::FORWARD>(const InterfaceState* state) {
return state->outgoingTrajectories();
}
template <>
inline const InterfaceState::Solutions& trajectories<Interface::BACKWARD>(const InterfaceState* state) {
return state->incomingTrajectories();
}
} // namespace task_constructor
} // namespace moveit

View File

@ -288,19 +288,38 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
return os;
}
/** Collect all potential solutions originating from start */
template <Interface::Direction dir>
struct SolutionCollector
{
SolutionCollector(size_t max_depth) : max_depth(max_depth) {}
SolutionCollector(size_t max_depth, const SolutionBase& start) : max_depth(max_depth) {
trace.reserve(max_depth);
traverse(start, InterfaceState::Priority(0, 0.0));
assert(trace.empty());
}
void operator()(const SolutionSequence::container_type& trace, const InterfaceState::Priority& prio) {
assert(prio.depth() == trace.size());
assert(prio.depth() <= max_depth);
solutions.emplace_back(std::make_pair(trace, prio));
void traverse(const SolutionBase& start, const InterfaceState::Priority& prio) {
const InterfaceState::Solutions& next = trajectories<dir>(state<dir>(start));
if (next.empty()) { // when reaching the end, add the trace to solutions
assert(prio.depth() == trace.size());
assert(prio.depth() <= max_depth);
solutions.emplace_back(std::make_pair(trace, prio));
} else {
for (SolutionBase* successor : next) {
if (successor->isFailure())
continue;
trace.push_back(successor);
traverse(*successor, prio + InterfaceState::Priority(1, successor->cost()));
trace.pop_back();
}
}
}
using SolutionCostPairs = std::list<std::pair<SolutionSequence::container_type, InterfaceState::Priority>>;
SolutionCostPairs solutions;
const size_t max_depth;
SolutionSequence::container_type trace;
};
inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) {
@ -321,16 +340,9 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
assert(num_before < children.size()); // creator should be one of our children
num_after = children.size() - 1 - num_before;
SolutionSequence::container_type trace;
trace.reserve(children.size());
// 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 paths starting at current solution
SolutionCollector outgoing(num_after);
traverse<Interface::FORWARD>(current, std::ref(outgoing), trace);
// find all incoming and outgoing solution paths originating from current solution
SolutionCollector<Interface::BACKWARD> incoming(num_before, current);
SolutionCollector<Interface::FORWARD> outgoing(num_after, current);
// collect (and sort) all solutions spanning from start to end of this container
ordered<SolutionSequencePtr> sorted;
@ -515,23 +527,6 @@ void SerialContainer::compute() {
}
}
template <Interface::Direction dir>
void SerialContainer::traverse(const SolutionBase& start, const SolutionProcessor& cb,
SolutionSequence::container_type& trace, const InterfaceState::Priority& prio) {
const InterfaceState::Solutions& solutions = start.trajectories<dir>();
if (solutions.empty()) // if we reached the end, call the callback
cb(trace, prio);
else
for (SolutionBase* successor : solutions) {
if (successor->isFailure())
continue;
trace.push_back(successor);
traverse<dir>(*successor, cb, trace, prio + InterfaceState::Priority(1, successor->cost()));
trace.pop_back();
}
}
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
: ContainerBasePrivate(me, name) {}