diff --git a/include/moveit_task_constructor/container.h b/include/moveit_task_constructor/container.h index a37a37a0..fd9e4abb 100644 --- a/include/moveit_task_constructor/container.h +++ b/include/moveit_task_constructor/container.h @@ -96,7 +96,7 @@ protected: private: /// append s as SubTrajectories to solution (lifted from private API) - virtual void append(const SolutionBase& s, std::vector& solution) const = 0; + virtual void append(const SolutionBase& s, SolutionTrajectory& solution) const = 0; /// callback for new solutions virtual void onNewSolution(SolutionBase &s) = 0; }; diff --git a/include/moveit_task_constructor/stage.h b/include/moveit_task_constructor/stage.h index 3ab87d8e..5b4b2ed8 100644 --- a/include/moveit_task_constructor/stage.h +++ b/include/moveit_task_constructor/stage.h @@ -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 SolutionTrajectory; /// exception thrown by Stage::init() diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 9ffd353c..7850f1e9 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -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& solution, + typedef std::function SolutionProcessor; /// process all solutions void processSolutions(const Task::SolutionProcessor &processor) const; - static std::vector& - flatten(const SolutionBase &s, std::vector& 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& solution) const override; + void append(const SolutionBase& s, SolutionTrajectory& solution) const override; void onNewSolution(SolutionBase &s) override; private: diff --git a/src/container_p.h b/src/container_p.h index 2efe5445..70d1fc70 100644 --- a/src/container_p.h +++ b/src/container_p.h @@ -50,7 +50,7 @@ public: explicit SerialSolution(StagePrivate* creator, std::vector&& subsolutions, double cost) : SolutionBase(creator, cost), subsolutions_(subsolutions) {} - void appendTo(std::vector& solution) const; + void appendTo(SolutionTrajectory& solution) const; private: // series of sub solutions @@ -68,7 +68,7 @@ public: void storeNewSolution(std::vector &&s, double cost); const std::list& solutions() const { return solutions_; } - void append(const SolutionBase& s, std::vector& solution) const override { + void append(const SolutionBase& s, SolutionTrajectory& solution) const override { assert(s.creator() == this); static_cast(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& solution) const; + void append(const SolutionBase& s, SolutionTrajectory& solution) const; void onNewSolution(SolutionBase &s) override; protected: diff --git a/src/debug.cpp b/src/debug.cpp index 67de3c82..ca49780c 100644 --- a/src/debug.cpp +++ b/src/debug.cpp @@ -9,7 +9,7 @@ namespace moveit { namespace task_constructor { -bool publishSolution(ros::Publisher& pub, const std::vector& 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 solution; + SolutionTrajectory solution; Task::flatten(s, solution); publishSolution(pub_, solution, s.cost(), false); diff --git a/src/stage_p.h b/src/stage_p.h index f8e2a0a3..0d45c2f1 100644 --- a/src/stage_p.h +++ b/src/stage_p.h @@ -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& 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& solution) const override { + void append(const SolutionBase& s, SolutionTrajectory& solution) const override { assert(s.creator() == this); solution.push_back(static_cast(&s)); } diff --git a/src/task.cpp b/src/task.cpp index 848db0c6..822d98f3 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -187,7 +187,7 @@ void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) c } void Task::processSolutions(const Task::SolutionProcessor& processor) const { - std::vector solution; + SolutionTrajectory solution; processSolutions([&solution, &processor](const SolutionBase& s) { solution.clear(); Task::flatten(s, solution);