mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
typedef SerialContainer::solution_container = std::vector<const SolutionBase*>
This commit is contained in:
parent
47e074ce7d
commit
8e1dfc6768
@ -54,20 +54,23 @@ public:
|
||||
size_t numSolutions() const override;
|
||||
void processSolutions(const SolutionProcessor &processor) const;
|
||||
|
||||
/// container used to represent a serial solution
|
||||
typedef std::vector<const SolutionBase*> solution_container;
|
||||
|
||||
protected:
|
||||
/// function type used for traversing solutions
|
||||
/// For each sub solution (current), the trace from the start as well as the
|
||||
/// accumulated cost of all solutions in the trace are provided.
|
||||
/// Return true, if traversal should continue, otherwise false.
|
||||
typedef std::function<bool(const SolutionBase& current,
|
||||
const std::vector<const SolutionBase*>& trace,
|
||||
const solution_container& trace,
|
||||
double trace_accumulated_cost)> SolutionProcessor;
|
||||
|
||||
/// traverse all solutions, starting at start and call the callback for each subsolution
|
||||
/// The return value is always false, indicating that the traversal eventually stopped.
|
||||
template<TraverseDirection dir>
|
||||
bool traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
||||
std::vector<const SolutionBase*> &trace, double trace_cost = 0);
|
||||
solution_container &trace, double trace_cost = 0);
|
||||
|
||||
protected:
|
||||
SerialContainer(SerialContainerPrivate* impl);
|
||||
|
||||
@ -151,7 +151,7 @@ inline ContainerBasePrivate::const_iterator SerialContainerPrivate::next(const_i
|
||||
struct SolutionCollector {
|
||||
SolutionCollector(const Stage::pointer& stage) : stopping_stage(stage->pimpl()) {}
|
||||
|
||||
bool operator()(const SolutionBase& current, const std::vector<const SolutionBase*>& trace, double cost) {
|
||||
bool operator()(const SolutionBase& current, const SerialContainer::solution_container& trace, double cost) {
|
||||
if (current.creator() != stopping_stage)
|
||||
return true; // not yet traversed to stopping_stage
|
||||
|
||||
@ -159,7 +159,7 @@ struct SolutionCollector {
|
||||
return false; // we are done
|
||||
}
|
||||
|
||||
std::list<std::pair<std::vector<const SolutionBase*>, double>> solutions;
|
||||
std::list<std::pair<SerialContainer::solution_container, double>> solutions;
|
||||
const StagePrivate* const stopping_stage;
|
||||
};
|
||||
|
||||
@ -175,7 +175,7 @@ void SerialContainerPrivate::onNewSolution(SolutionBase ¤t)
|
||||
SerialContainer *me = static_cast<SerialContainer*>(me_);
|
||||
|
||||
// TODO: can we get rid of this and use a temporary when calling traverse()?
|
||||
std::vector<const SolutionBase*> trace; trace.reserve(children().size());
|
||||
SerialContainer::solution_container trace; trace.reserve(children().size());
|
||||
|
||||
// find all incoming trajectories connected to s
|
||||
SolutionCollector incoming(children().front());
|
||||
@ -193,7 +193,7 @@ void SerialContainerPrivate::onNewSolution(SolutionBase ¤t)
|
||||
std::cerr << "new solution for: " << name() << std::endl;
|
||||
|
||||
// add solutions for all combinations of incoming + s + outgoing
|
||||
std::vector<const SolutionBase*> solution;
|
||||
SerialContainer::solution_container solution;
|
||||
solution.reserve(children().size());
|
||||
for (auto& in : incoming.solutions) {
|
||||
for (auto& out : outgoing.solutions) {
|
||||
@ -211,7 +211,7 @@ void SerialContainerPrivate::onNewSolution(SolutionBase ¤t)
|
||||
}
|
||||
}
|
||||
|
||||
void SerialContainerPrivate::storeNewSolution(std::vector<const SolutionBase*> &&s, double cost)
|
||||
void SerialContainerPrivate::storeNewSolution(SerialContainer::solution_container &&s, double cost)
|
||||
{
|
||||
assert(!s.empty());
|
||||
const InterfaceState *internal_from = s.front()->start();
|
||||
@ -360,7 +360,7 @@ void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &p
|
||||
|
||||
template <TraverseDirection dir>
|
||||
bool SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
||||
std::vector<const SolutionBase *> &trace, double trace_cost)
|
||||
solution_container &trace, double trace_cost)
|
||||
{
|
||||
if (!cb(start, trace, trace_cost))
|
||||
// stopping criterium met: stop traversal along dir
|
||||
|
||||
@ -47,14 +47,14 @@ private:
|
||||
|
||||
class SerialSolution : public SolutionBase {
|
||||
public:
|
||||
explicit SerialSolution(StagePrivate* creator, std::vector<const SolutionBase*>&& subsolutions, double cost)
|
||||
explicit SerialSolution(StagePrivate* creator, SerialContainer::solution_container&& subsolutions, double cost)
|
||||
: SolutionBase(creator, cost), subsolutions_(subsolutions)
|
||||
{}
|
||||
void appendTo(SolutionTrajectory& solution) const;
|
||||
|
||||
private:
|
||||
// series of sub solutions
|
||||
std::vector<const SolutionBase*> subsolutions_;
|
||||
SerialContainer::solution_container subsolutions_;
|
||||
};
|
||||
|
||||
|
||||
@ -65,7 +65,7 @@ public:
|
||||
SerialContainerPrivate(SerialContainer* me, const std::string &name);
|
||||
|
||||
void onNewSolution(SolutionBase &s) override;
|
||||
void storeNewSolution(std::vector<const SolutionBase *> &&s, double cost);
|
||||
void storeNewSolution(SerialContainer::solution_container &&s, double cost);
|
||||
const std::list<SerialSolution>& solutions() const { return solutions_; }
|
||||
|
||||
void append(const SolutionBase& s, SolutionTrajectory& solution) const override {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user