mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
SerialSolution -> SolutionSequence
This commit is contained in:
parent
a82a48bb70
commit
9ceecd235f
@ -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);
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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 ¤t)
|
||||
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 ¤t)
|
||||
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 ¤t)
|
||||
// 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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
} }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user