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;
|
size_t numSolutions() const override;
|
||||||
void processSolutions(const SolutionProcessor &processor) const override;
|
void processSolutions(const SolutionProcessor &processor) const override;
|
||||||
|
|
||||||
/// container used to represent a serial solution
|
|
||||||
typedef std::vector<const SolutionBase*> solution_container;
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
/// called by a (direct) child when a new solution becomes available
|
/// called by a (direct) child when a new solution becomes available
|
||||||
void onNewSolution(const SolutionBase &s) override;
|
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;
|
double trace_accumulated_cost)> SolutionProcessor;
|
||||||
|
|
||||||
/// Traverse all solution pathes starting at start and going in given direction dir
|
/// 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
|
/// the full trace (from start to end, but not including start) and its accumulated costs
|
||||||
template<Interface::Direction dir>
|
template<Interface::Direction dir>
|
||||||
void traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
void traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
||||||
solution_container &trace, double trace_cost = 0);
|
SolutionSequence::container_type &trace, double trace_cost = 0);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
SerialContainer(SerialContainerPrivate* impl);
|
SerialContainer(SerialContainerPrivate* impl);
|
||||||
|
|||||||
@ -136,27 +136,6 @@ protected:
|
|||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(ContainerBase)
|
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.
|
/* 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.
|
* The solution of a single child stage is usually disconnected to the container's start or end.
|
||||||
@ -188,7 +167,7 @@ private:
|
|||||||
InterfaceFlags accepted);
|
InterfaceFlags accepted);
|
||||||
|
|
||||||
// set of all solutions
|
// set of all solutions
|
||||||
ordered<SerialSolution> solutions_;
|
ordered<SolutionSequence> solutions_;
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(SerialContainer)
|
PIMPL_FUNCTIONS(SerialContainer)
|
||||||
|
|
||||||
|
|||||||
@ -172,11 +172,9 @@ private:
|
|||||||
|
|
||||||
|
|
||||||
class StagePrivate;
|
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 {
|
class SolutionBase {
|
||||||
public:
|
public:
|
||||||
inline const InterfaceState* start() const { return start_; }
|
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 {
|
class SubTrajectory : public SolutionBase {
|
||||||
public:
|
public:
|
||||||
SubTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory = robot_trajectory::RobotTrajectoryPtr())
|
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
|
template <> inline
|
||||||
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const {
|
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const {
|
||||||
return end_->outgoingTrajectories();
|
return end_->outgoingTrajectories();
|
||||||
|
|||||||
@ -283,18 +283,18 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
|
|||||||
struct SolutionCollector {
|
struct SolutionCollector {
|
||||||
SolutionCollector(size_t max_depth) : max_depth(max_depth) {}
|
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
|
// traced path should not extend past container boundaries
|
||||||
assert(trace.size() <= max_depth);
|
assert(trace.size() <= max_depth);
|
||||||
solutions.emplace_back(std::make_pair(trace, cost));
|
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;
|
SolutionCostPairs solutions;
|
||||||
const size_t max_depth;
|
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) {
|
const InterfaceState::Priority &prio) {
|
||||||
for (const SolutionBase* solution : partial_solution_path) {
|
for (const SolutionBase* solution : partial_solution_path) {
|
||||||
// here it suffices to update the start state, because the end state is the start state
|
// 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
|
assert(num_before < children.size()); // creator should be one of our children
|
||||||
num_after = children.size()-1 - num_before;
|
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
|
// find all incoming solution paths ending at current solution
|
||||||
SolutionCollector incoming(num_before);
|
SolutionCollector incoming(num_before);
|
||||||
@ -333,8 +333,8 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
|||||||
traverse<Interface::FORWARD>(current, std::ref(outgoing), trace);
|
traverse<Interface::FORWARD>(current, std::ref(outgoing), trace);
|
||||||
|
|
||||||
// collect (and sort) all solutions spanning from start to end of this container
|
// collect (and sort) all solutions spanning from start to end of this container
|
||||||
ordered<SerialSolution> sorted;
|
ordered<SolutionSequence> sorted;
|
||||||
SerialContainer::solution_container solution;
|
SolutionSequence::container_type solution;
|
||||||
solution.reserve(children.size());
|
solution.reserve(children.size());
|
||||||
for (auto& in : incoming.solutions) {
|
for (auto& in : incoming.solutions) {
|
||||||
for (auto& out : outgoing.solutions) {
|
for (auto& out : outgoing.solutions) {
|
||||||
@ -352,7 +352,7 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
|||||||
// insert outgoing solutions in normal order
|
// insert outgoing solutions in normal order
|
||||||
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
||||||
// store solution in sorted list
|
// 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) {
|
} else if (prio.depth() > 1) {
|
||||||
// update state priorities along the whole partial solution path
|
// update state priorities along the whole partial solution path
|
||||||
updateStateCosts(in.first, prio);
|
updateStateCosts(in.first, prio);
|
||||||
@ -655,7 +655,7 @@ void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &p
|
|||||||
|
|
||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
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>();
|
const InterfaceState::Solutions& solutions = start.trajectories<dir>();
|
||||||
if (solutions.empty()) // if we reached the end, call the callback
|
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,
|
void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution &solution,
|
||||||
Introspection *introspection) const
|
Introspection *introspection) const
|
||||||
|
|||||||
@ -127,7 +127,6 @@ void SolutionBase::setCost(double cost) {
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
||||||
Introspection *introspection) const {
|
Introspection *introspection) const {
|
||||||
msg.sub_trajectory.emplace_back();
|
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