mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Simplify SolutionCollector
This commit is contained in:
parent
576a1aa48d
commit
c8e75ab205
@ -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);
|
||||
};
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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) {}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user