diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index ebdc119f..35a903ad 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -187,27 +187,6 @@ protected: }; PIMPL_FUNCTIONS(SerialContainer) -/** Wrap an existing solution - for use in parallel containers and wrappers. - * - * This essentially wraps a solution of a child and thus allows - * for new clones of start / end states, which in turn will - * have separate incoming/outgoing trajectories */ -class WrappedSolution : public SolutionBase -{ -public: - explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped, double cost, std::string comment) - : SolutionBase(creator, cost, std::move(comment)), wrapped_(wrapped) {} - explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped, double cost) - : SolutionBase(creator, cost), wrapped_(wrapped) {} - explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped) - : WrappedSolution(creator, wrapped, wrapped->cost()) {} - void fillMessage(moveit_task_constructor_msgs::Solution& solution, - Introspection* introspection = nullptr) const override; - -private: - const SolutionBase* wrapped_; -}; - class ParallelContainerBasePrivate : public ContainerBasePrivate { friend class ParallelContainerBase; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index c57cd426..f4e30104 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -312,6 +312,29 @@ private: }; MOVEIT_CLASS_FORWARD(SolutionSequence) +/** Wrap an existing solution + * + * used by parallel containers and wrappers. + * + * This essentially wraps a solution of a child and thus allows + * for new clones of start / end states, which in turn will + * have separate incoming/outgoing trajectories */ +class WrappedSolution : public SolutionBase +{ +public: + explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped, double cost, std::string comment) + : SolutionBase(creator, cost, std::move(comment)), wrapped_(wrapped) {} + explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped, double cost) + : SolutionBase(creator, cost), wrapped_(wrapped) {} + explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped) + : WrappedSolution(creator, wrapped, wrapped->cost()) {} + void fillMessage(moveit_task_constructor_msgs::Solution& solution, + Introspection* introspection = nullptr) const override; + +private: + const SolutionBase* wrapped_; +}; + template <> inline const InterfaceState::Solutions& SolutionBase::trajectories() const { return end_->outgoingTrajectories(); diff --git a/core/src/container.cpp b/core/src/container.cpp index 7655f3a4..2a089456 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -554,17 +554,6 @@ void SerialContainer::traverse(const SolutionBase& start, const SolutionProcesso } } -void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, - Introspection* introspection) const { - wrapped_->fillMessage(solution, introspection); - - // prepend this solutions info as a SubSolution msg - moveit_task_constructor_msgs::SubSolution sub_msg; - SolutionBase::fillInfo(sub_msg.info, introspection); - sub_msg.sub_solution_id.push_back(introspection ? introspection->solutionId(*wrapped_) : 0); - solution.sub_solution.insert(solution.sub_solution.begin(), std::move(sub_msg)); -} - ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name) : ContainerBasePrivate(me, name) {} diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 642063bc..ed53b4ec 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -206,5 +206,16 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, } } } + +void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, + Introspection* introspection) const { + wrapped_->fillMessage(solution, introspection); + + // prepend this solutions info as a SubSolution msg + moveit_task_constructor_msgs::SubSolution sub_msg; + SolutionBase::fillInfo(sub_msg.info, introspection); + sub_msg.sub_solution_id.push_back(introspection ? introspection->solutionId(*wrapped_) : 0); + solution.sub_solution.insert(solution.sub_solution.begin(), std::move(sub_msg)); +} } // namespace task_constructor } // namespace moveit