SerialSolution -> SolutionSequence

This commit is contained in:
Robert Haschke 2018-03-12 22:11:14 +01:00
parent a82a48bb70
commit 9ceecd235f
5 changed files with 71 additions and 62 deletions

View File

@ -107,14 +107,11 @@ public:
size_t numSolutions() const override;
void processSolutions(const SolutionProcessor &processor) const override;
/// container used to represent a serial solution
typedef std::vector<const SolutionBase*> solution_container;
protected:
/// called by a (direct) child when a new solution becomes available
void onNewSolution(const SolutionBase &s) override;
typedef std::function<void(const solution_container& trace,
typedef std::function<void(const SolutionSequence::container_type &trace,
double trace_accumulated_cost)> SolutionProcessor;
/// Traverse all solution pathes starting at start and going in given direction dir
@ -123,7 +120,7 @@ protected:
/// 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,
solution_container &trace, double trace_cost = 0);
SolutionSequence::container_type &trace, double trace_cost = 0);
protected:
SerialContainer(SerialContainerPrivate* impl);

View File

@ -136,27 +136,6 @@ protected:
};
PIMPL_FUNCTIONS(ContainerBase)
/** Representation of a single, full solution path of a SerialContainer.
*
* A serial solution describes a full solution path through all children
* of a SerialContainer. This is a vector (of children().size()) of pointers
* to all solutions of the children. Hence, we don't need to copy those solutions. */
class SerialSolution : public SolutionBase {
public:
explicit SerialSolution(StagePrivate* creator, SerialContainer::solution_container&& subsolutions, double cost)
: SolutionBase(creator, cost), subsolutions_(subsolutions)
{}
/// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection *introspection) const override;
inline const InterfaceState* internalStart() const { return subsolutions_.front()->start(); }
inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); }
private:
/// series of sub solutions
SerialContainer::solution_container subsolutions_;
};
/* A solution of a SerialContainer needs to connect start to end via a full path.
* The solution of a single child stage is usually disconnected to the container's start or end.
@ -188,7 +167,7 @@ private:
InterfaceFlags accepted);
// set of all solutions
ordered<SerialSolution> solutions_;
ordered<SolutionSequence> solutions_;
};
PIMPL_FUNCTIONS(SerialContainer)

View File

@ -172,11 +172,9 @@ private:
class StagePrivate;
class SubTrajectory;
/// SolutionTrajectory is composed of a series of SubTrajectories
typedef std::vector<const SubTrajectory*> SolutionTrajectory;
/// abstract base class for solutions (primitive and sequences)
class SolutionBase {
public:
inline const InterfaceState* start() const { return start_; }
@ -241,7 +239,7 @@ private:
};
// SubTrajectory connects interface states of ComputeStages
/// SubTrajectory connects interface states of ComputeStages
class SubTrajectory : public SolutionBase {
public:
SubTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory = robot_trajectory::RobotTrajectoryPtr())
@ -260,6 +258,35 @@ private:
};
/** Sequence of individual sub solutions
*
* A solution sequence describes a solution that is composed from several individual
* sub solutions that need to be chained together to yield the overall solutions.
*/
class SolutionSequence : public SolutionBase {
public:
typedef std::vector<const SolutionBase*> container_type;
explicit SolutionSequence()
: SolutionBase()
{}
SolutionSequence(container_type&& subsolutions, double cost = 0.0, StagePrivate* creator = nullptr)
: SolutionBase(creator, cost), subsolutions_(subsolutions)
{}
void push_back(const SolutionBase& solution);
/// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection *introspection) const override;
inline const InterfaceState* internalStart() const { return subsolutions_.front()->start(); }
inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); }
private:
/// series of sub solutions
container_type subsolutions_;
};
template <> inline
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const {
return end_->outgoingTrajectories();

View File

@ -283,18 +283,18 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
struct SolutionCollector {
SolutionCollector(size_t max_depth) : max_depth(max_depth) {}
void operator()(const SerialContainer::solution_container& trace, double cost) {
void operator()(const SolutionSequence::container_type& trace, double cost) {
// traced path should not extend past container boundaries
assert(trace.size() <= max_depth);
solutions.emplace_back(std::make_pair(trace, cost));
}
typedef std::list<std::pair<SerialContainer::solution_container, double>> SolutionCostPairs;
typedef std::list<std::pair<SolutionSequence::container_type, double>> SolutionCostPairs;
SolutionCostPairs solutions;
const size_t max_depth;
};
void updateStateCosts(const SerialContainer::solution_container &partial_solution_path,
void updateStateCosts(const SolutionSequence::container_type &partial_solution_path,
const InterfaceState::Priority &prio) {
for (const SolutionBase* solution : partial_solution_path) {
// here it suffices to update the start state, because the end state is the start state
@ -322,7 +322,7 @@ 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;
SerialContainer::solution_container trace; trace.reserve(children.size());
SolutionSequence::container_type trace; trace.reserve(children.size());
// find all incoming solution paths ending at current solution
SolutionCollector incoming(num_before);
@ -333,8 +333,8 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
traverse<Interface::FORWARD>(current, std::ref(outgoing), trace);
// collect (and sort) all solutions spanning from start to end of this container
ordered<SerialSolution> sorted;
SerialContainer::solution_container solution;
ordered<SolutionSequence> sorted;
SolutionSequence::container_type solution;
solution.reserve(children.size());
for (auto& in : incoming.solutions) {
for (auto& out : outgoing.solutions) {
@ -352,7 +352,7 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
// insert outgoing solutions in normal order
solution.insert(solution.end(), out.first.begin(), out.first.end());
// store solution in sorted list
sorted.insert(SerialSolution(impl, std::move(solution), prio.cost()));
sorted.insert(SolutionSequence(std::move(solution), prio.cost(), impl));
} else if (prio.depth() > 1) {
// update state priorities along the whole partial solution path
updateStateCosts(in.first, prio);
@ -655,7 +655,7 @@ void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &p
template <Interface::Direction dir>
void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb,
solution_container &trace, double trace_cost)
SolutionSequence::container_type &trace, double trace_cost)
{
const InterfaceState::Solutions& solutions = start.trajectories<dir>();
if (solutions.empty()) // if we reached the end, call the callback
@ -671,28 +671,6 @@ void SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso
}
}
void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
Introspection* introspection) const
{
moveit_task_constructor_msgs::SubSolution sub_msg;
sub_msg.id = introspection ? introspection->solutionId(*this) : 0;
sub_msg.cost = this->cost();
const Introspection *ci = introspection;
sub_msg.stage_id = ci ? ci->stageId(this->creator()->me()) : 0;
sub_msg.sub_solution_id.reserve(subsolutions_.size());
if (introspection) {
for (const SolutionBase* s : subsolutions_)
sub_msg.sub_solution_id.push_back(introspection->solutionId(*s));
msg.sub_solution.push_back(sub_msg);
}
msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size());
for (const SolutionBase* s : subsolutions_)
s->fillMessage(msg, introspection);
}
void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution &solution,
Introspection *introspection) const

View File

@ -127,7 +127,6 @@ void SolutionBase::setCost(double cost) {
void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg,
Introspection *introspection) const {
msg.sub_trajectory.emplace_back();
@ -150,4 +149,33 @@ void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg,
}
void SolutionSequence::push_back(const SolutionBase& solution)
{
subsolutions_.push_back(&solution);
}
void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution &msg,
Introspection* introspection) const
{
moveit_task_constructor_msgs::SubSolution sub_msg;
sub_msg.id = introspection ? introspection->solutionId(*this) : 0;
sub_msg.cost = this->cost();
const Introspection *ci = introspection;
sub_msg.stage_id = ci ? ci->stageId(this->creator()->me()) : 0;
sub_msg.sub_solution_id.reserve(subsolutions_.size());
if (introspection) {
for (const SolutionBase* s : subsolutions_)
sub_msg.sub_solution_id.push_back(introspection->solutionId(*s));
msg.sub_solution.push_back(sub_msg);
}
msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size());
for (const SolutionBase* s : subsolutions_)
s->fillMessage(msg, introspection);
}
} }