mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
typedef SolutionTrajectory = std::vector<const SubTrajectory*>
This commit is contained in:
parent
4b56770d50
commit
47e074ce7d
@ -96,7 +96,7 @@ protected:
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
/// append s as SubTrajectories to solution (lifted from private API)
|
/// 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
|
/// callback for new solutions
|
||||||
virtual void onNewSolution(SolutionBase &s) = 0;
|
virtual void onNewSolution(SolutionBase &s) = 0;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -48,6 +48,8 @@ private:
|
|||||||
// trajectories could have a name, e.g. a generator could name its solutions
|
// trajectories could have a name, e.g. a generator could name its solutions
|
||||||
std::string name_;
|
std::string name_;
|
||||||
};
|
};
|
||||||
|
/// SolutionTrajectory is composed of a series of SubTrajectories
|
||||||
|
typedef std::vector<const SubTrajectory*> SolutionTrajectory;
|
||||||
|
|
||||||
|
|
||||||
/// exception thrown by Stage::init()
|
/// exception thrown by Stage::init()
|
||||||
|
|||||||
@ -47,20 +47,19 @@ public:
|
|||||||
/// For each solution, composed from several SubTrajectories,
|
/// For each solution, composed from several SubTrajectories,
|
||||||
/// the vector of SubTrajectories as well as the associated costs are provided.
|
/// the vector of SubTrajectories as well as the associated costs are provided.
|
||||||
/// Return true, if traversal should continue, otherwise false.
|
/// 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;
|
double accumulated_cost)> SolutionProcessor;
|
||||||
/// process all solutions
|
/// process all solutions
|
||||||
void processSolutions(const Task::SolutionProcessor &processor) const;
|
void processSolutions(const Task::SolutionProcessor &processor) const;
|
||||||
|
|
||||||
static std::vector<const SubTrajectory*>&
|
static SolutionTrajectory& flatten(const SolutionBase &s, SolutionTrajectory& result);
|
||||||
flatten(const SolutionBase &s, std::vector<const SubTrajectory*>& result);
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
void processSolutions(const ContainerBase::SolutionProcessor &processor) const 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;
|
void onNewSolution(SolutionBase &s) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|||||||
@ -50,7 +50,7 @@ public:
|
|||||||
explicit SerialSolution(StagePrivate* creator, std::vector<const SolutionBase*>&& subsolutions, double cost)
|
explicit SerialSolution(StagePrivate* creator, std::vector<const SolutionBase*>&& subsolutions, double cost)
|
||||||
: SolutionBase(creator, cost), subsolutions_(subsolutions)
|
: SolutionBase(creator, cost), subsolutions_(subsolutions)
|
||||||
{}
|
{}
|
||||||
void appendTo(std::vector<const SubTrajectory*>& solution) const;
|
void appendTo(SolutionTrajectory& solution) const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// series of sub solutions
|
// series of sub solutions
|
||||||
@ -68,7 +68,7 @@ public:
|
|||||||
void storeNewSolution(std::vector<const SolutionBase *> &&s, double cost);
|
void storeNewSolution(std::vector<const SolutionBase *> &&s, double cost);
|
||||||
const std::list<SerialSolution>& solutions() const { return solutions_; }
|
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);
|
assert(s.creator() == this);
|
||||||
static_cast<const SerialSolution&>(s).appendTo(solution);
|
static_cast<const SerialSolution&>(s).appendTo(solution);
|
||||||
}
|
}
|
||||||
@ -104,7 +104,7 @@ class WrapperBasePrivate : public ContainerBasePrivate {
|
|||||||
friend class WrapperBase;
|
friend class WrapperBase;
|
||||||
public:
|
public:
|
||||||
// these methods are lifted to the public API
|
// 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;
|
void onNewSolution(SolutionBase &s) override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|||||||
@ -9,7 +9,7 @@
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
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())
|
if (solution.empty())
|
||||||
return true;
|
return true;
|
||||||
|
|
||||||
@ -56,7 +56,7 @@ NewSolutionPublisher::NewSolutionPublisher(const std::string &topic)
|
|||||||
void NewSolutionPublisher::operator()(const SolutionBase &s)
|
void NewSolutionPublisher::operator()(const SolutionBase &s)
|
||||||
{
|
{
|
||||||
// flatten s into vector of SubTrajectories
|
// flatten s into vector of SubTrajectories
|
||||||
std::vector<const SubTrajectory*> solution;
|
SolutionTrajectory solution;
|
||||||
Task::flatten(s, solution);
|
Task::flatten(s, solution);
|
||||||
|
|
||||||
publishSolution(pub_, solution, s.cost(), false);
|
publishSolution(pub_, solution, s.cost(), false);
|
||||||
|
|||||||
@ -60,7 +60,7 @@ public:
|
|||||||
inline void setPrevEnds(const InterfacePtr& prev_ends) { prev_ends_ = prev_ends; }
|
inline void setPrevEnds(const InterfacePtr& prev_ends) { prev_ends_ = prev_ends; }
|
||||||
inline void setNextStarts(const InterfacePtr& next_starts) { next_starts_ = next_starts; }
|
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:
|
protected:
|
||||||
Stage* const me_; // associated/owning Stage instance
|
Stage* const me_; // associated/owning Stage instance
|
||||||
@ -89,7 +89,7 @@ public:
|
|||||||
ComputeBasePrivate(Stage* me, const std::string& name)
|
ComputeBasePrivate(Stage* me, const std::string& name)
|
||||||
: StagePrivate(me, 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);
|
assert(s.creator() == this);
|
||||||
solution.push_back(static_cast<const SubTrajectory*>(&s));
|
solution.push_back(static_cast<const SubTrajectory*>(&s));
|
||||||
}
|
}
|
||||||
|
|||||||
@ -187,7 +187,7 @@ void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) c
|
|||||||
}
|
}
|
||||||
|
|
||||||
void Task::processSolutions(const Task::SolutionProcessor& processor) const {
|
void Task::processSolutions(const Task::SolutionProcessor& processor) const {
|
||||||
std::vector<const SubTrajectory*> solution;
|
SolutionTrajectory solution;
|
||||||
processSolutions([&solution, &processor](const SolutionBase& s) {
|
processSolutions([&solution, &processor](const SolutionBase& s) {
|
||||||
solution.clear();
|
solution.clear();
|
||||||
Task::flatten(s, solution);
|
Task::flatten(s, solution);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user