typedef SolutionTrajectory = std::vector<const SubTrajectory*>

This commit is contained in:
Robert Haschke 2017-10-20 16:21:45 +02:00
parent 4b56770d50
commit 47e074ce7d
7 changed files with 14 additions and 13 deletions

View File

@ -96,7 +96,7 @@ protected:
private:
/// append s as SubTrajectories to solution (lifted from private API)
virtual void append(const SolutionBase& s, std::vector<const SubTrajectory*>& solution) const = 0;
virtual void append(const SolutionBase& s, SolutionTrajectory& solution) const = 0;
/// callback for new solutions
virtual void onNewSolution(SolutionBase &s) = 0;
};

View File

@ -48,6 +48,8 @@ private:
// trajectories could have a name, e.g. a generator could name its solutions
std::string name_;
};
/// SolutionTrajectory is composed of a series of SubTrajectories
typedef std::vector<const SubTrajectory*> SolutionTrajectory;
/// exception thrown by Stage::init()

View File

@ -47,20 +47,19 @@ public:
/// For each solution, composed from several SubTrajectories,
/// the vector of SubTrajectories as well as the associated costs are provided.
/// Return true, if traversal should continue, otherwise false.
typedef std::function<bool(const std::vector<const SubTrajectory*>& solution,
typedef std::function<bool(const SolutionTrajectory& solution,
double accumulated_cost)> SolutionProcessor;
/// process all solutions
void processSolutions(const Task::SolutionProcessor &processor) const;
static std::vector<const SubTrajectory*>&
flatten(const SolutionBase &s, std::vector<const SubTrajectory*>& result);
static SolutionTrajectory& flatten(const SolutionBase &s, SolutionTrajectory& result);
protected:
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
bool canCompute() const override;
bool compute() override;
void processSolutions(const ContainerBase::SolutionProcessor &processor) const override;
void append(const SolutionBase& s, std::vector<const SubTrajectory*>& solution) const override;
void append(const SolutionBase& s, SolutionTrajectory& solution) const override;
void onNewSolution(SolutionBase &s) override;
private:

View File

@ -50,7 +50,7 @@ public:
explicit SerialSolution(StagePrivate* creator, std::vector<const SolutionBase*>&& subsolutions, double cost)
: SolutionBase(creator, cost), subsolutions_(subsolutions)
{}
void appendTo(std::vector<const SubTrajectory*>& solution) const;
void appendTo(SolutionTrajectory& solution) const;
private:
// series of sub solutions
@ -68,7 +68,7 @@ public:
void storeNewSolution(std::vector<const SolutionBase *> &&s, double cost);
const std::list<SerialSolution>& solutions() const { return solutions_; }
void append(const SolutionBase& s, std::vector<const SubTrajectory*>& solution) const override {
void append(const SolutionBase& s, SolutionTrajectory& solution) const override {
assert(s.creator() == this);
static_cast<const SerialSolution&>(s).appendTo(solution);
}
@ -104,7 +104,7 @@ class WrapperBasePrivate : public ContainerBasePrivate {
friend class WrapperBase;
public:
// these methods are lifted to the public API
void append(const SolutionBase& s, std::vector<const SubTrajectory*>& solution) const;
void append(const SolutionBase& s, SolutionTrajectory& solution) const;
void onNewSolution(SolutionBase &s) override;
protected:

View File

@ -9,7 +9,7 @@
namespace moveit { namespace task_constructor {
bool publishSolution(ros::Publisher& pub, const std::vector<const SubTrajectory*>& solution, double cost, bool wait){
bool publishSolution(ros::Publisher& pub, const SolutionTrajectory& solution, double cost, bool wait){
if (solution.empty())
return true;
@ -56,7 +56,7 @@ NewSolutionPublisher::NewSolutionPublisher(const std::string &topic)
void NewSolutionPublisher::operator()(const SolutionBase &s)
{
// flatten s into vector of SubTrajectories
std::vector<const SubTrajectory*> solution;
SolutionTrajectory solution;
Task::flatten(s, solution);
publishSolution(pub_, solution, s.cost(), false);

View File

@ -60,7 +60,7 @@ public:
inline void setPrevEnds(const InterfacePtr& prev_ends) { prev_ends_ = prev_ends; }
inline void setNextStarts(const InterfacePtr& next_starts) { next_starts_ = next_starts; }
virtual void append(const SolutionBase& s, std::vector<const SubTrajectory*>& solution) const = 0;
virtual void append(const SolutionBase& s, SolutionTrajectory& solution) const = 0;
protected:
Stage* const me_; // associated/owning Stage instance
@ -89,7 +89,7 @@ public:
ComputeBasePrivate(Stage* me, const std::string& name)
: StagePrivate(me, name)
{}
void append(const SolutionBase& s, std::vector<const SubTrajectory*>& solution) const override {
void append(const SolutionBase& s, SolutionTrajectory& solution) const override {
assert(s.creator() == this);
solution.push_back(static_cast<const SubTrajectory*>(&s));
}

View File

@ -187,7 +187,7 @@ void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) c
}
void Task::processSolutions(const Task::SolutionProcessor& processor) const {
std::vector<const SubTrajectory*> solution;
SolutionTrajectory solution;
processSolutions([&solution, &processor](const SolutionBase& s) {
solution.clear();
Task::flatten(s, solution);