expose WrappedSolution together with other Solution types

Moved out of private header.

Users can access these objects, so they should know about the type.
This commit is contained in:
v4hn 2020-08-18 18:47:43 +02:00
parent 7609f0b1e2
commit 4a6c389aee
4 changed files with 34 additions and 32 deletions

View File

@ -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;

View File

@ -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<Interface::FORWARD>() const {
return end_->outgoingTrajectories();

View File

@ -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) {}

View File

@ -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