diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index a9098c0d..b5bb0c43 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -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 solution_container; - protected: /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase &s) override; - typedef std::function 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 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); diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 79c184cf..e9439548 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -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 solutions_; + ordered solutions_; }; PIMPL_FUNCTIONS(SerialContainer) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index c91e2441..163c0017 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -172,11 +172,9 @@ private: class StagePrivate; -class SubTrajectory; -/// SolutionTrajectory is composed of a series of SubTrajectories -typedef std::vector 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 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() const { return end_->outgoingTrajectories(); diff --git a/core/src/container.cpp b/core/src/container.cpp index aa2ff79c..7b12e37e 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -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> SolutionCostPairs; + typedef std::list> 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(current, std::ref(outgoing), trace); // collect (and sort) all solutions spanning from start to end of this container - ordered sorted; - SerialContainer::solution_container solution; + ordered 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 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(); 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 diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 360ceb21..bd0b03d2 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -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); +} + + } }