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:
|
||||
/// 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;
|
||||
};
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user