mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
promise to not modify solutions
This commit is contained in:
parent
e9363919e8
commit
104e52eb48
@ -68,7 +68,7 @@ public:
|
|||||||
virtual bool compute() = 0;
|
virtual bool compute() = 0;
|
||||||
|
|
||||||
/// called by a (direct) child when a new solution becomes available
|
/// called by a (direct) child when a new solution becomes available
|
||||||
virtual void onNewSolution(SolutionBase& t) = 0;
|
virtual void onNewSolution(const SolutionBase& s) = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
ContainerBase(ContainerBasePrivate* impl);
|
ContainerBase(ContainerBasePrivate* impl);
|
||||||
@ -96,7 +96,7 @@ public:
|
|||||||
|
|
||||||
protected:
|
protected:
|
||||||
/// called by a (direct) child when a new solution becomes available
|
/// called by a (direct) child when a new solution becomes available
|
||||||
void onNewSolution(SolutionBase &s) override;
|
void onNewSolution(const SolutionBase &s) override;
|
||||||
|
|
||||||
/// function type used for traversing solutions
|
/// function type used for traversing solutions
|
||||||
/// For each sub solution (current), the trace from the start as well as the
|
/// For each sub solution (current), the trace from the start as well as the
|
||||||
@ -142,7 +142,7 @@ protected:
|
|||||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||||
|
|
||||||
/// called by a (direct) child when a new solution becomes available
|
/// called by a (direct) child when a new solution becomes available
|
||||||
void onNewSolution(SolutionBase &s) override;
|
void onNewSolution(const SolutionBase &s) override;
|
||||||
|
|
||||||
/// callback for new start states (received externally)
|
/// callback for new start states (received externally)
|
||||||
virtual void onNewStartState(const InterfaceState &external) = 0;
|
virtual void onNewStartState(const InterfaceState &external) = 0;
|
||||||
|
|||||||
@ -158,7 +158,7 @@ PIMPL_FUNCTIONS(SerialContainer)
|
|||||||
* have separate incoming/outgoing trajectories */
|
* have separate incoming/outgoing trajectories */
|
||||||
class WrappedSolution : public SolutionBase {
|
class WrappedSolution : public SolutionBase {
|
||||||
public:
|
public:
|
||||||
explicit WrappedSolution(StagePrivate* creator, SolutionBase* wrapped)
|
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped)
|
||||||
: SolutionBase(creator, wrapped->cost()), wrapped_(wrapped)
|
: SolutionBase(creator, wrapped->cost()), wrapped_(wrapped)
|
||||||
{}
|
{}
|
||||||
void fillMessage(moveit_task_constructor_msgs::Solution &solution,
|
void fillMessage(moveit_task_constructor_msgs::Solution &solution,
|
||||||
@ -167,7 +167,7 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
SolutionBase* wrapped_;
|
const SolutionBase* wrapped_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -92,7 +92,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; }
|
||||||
|
|
||||||
void newSolution(SolutionBase ¤t);
|
void newSolution(const SolutionBase ¤t);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
Stage* const me_; // associated/owning Stage instance
|
Stage* const me_; // associated/owning Stage instance
|
||||||
|
|||||||
@ -120,7 +120,7 @@ protected:
|
|||||||
void initScene();
|
void initScene();
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
void onNewSolution(SolutionBase &s) override;
|
void onNewSolution(const SolutionBase &s) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::string id_;
|
std::string id_;
|
||||||
|
|||||||
@ -233,7 +233,7 @@ struct SolutionCollector {
|
|||||||
const StagePrivate* const stopping_stage;
|
const StagePrivate* const stopping_stage;
|
||||||
};
|
};
|
||||||
|
|
||||||
void SerialContainer::onNewSolution(SolutionBase ¤t)
|
void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
||||||
{
|
{
|
||||||
const StagePrivate *creator = current.creator();
|
const StagePrivate *creator = current.creator();
|
||||||
auto& children = pimpl()->children();
|
auto& children = pimpl()->children();
|
||||||
@ -495,7 +495,7 @@ ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase
|
|||||||
}));
|
}));
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParallelContainerBase::onNewSolution(SolutionBase &s)
|
void ParallelContainerBase::onNewSolution(const SolutionBase &s)
|
||||||
{
|
{
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
WrappedSolution wrapped(impl, &s);
|
WrappedSolution wrapped(impl, &s);
|
||||||
|
|||||||
@ -102,7 +102,7 @@ void StagePrivate::validate() const {
|
|||||||
if (errors) throw errors;
|
if (errors) throw errors;
|
||||||
}
|
}
|
||||||
|
|
||||||
void StagePrivate::newSolution(SolutionBase &solution)
|
void StagePrivate::newSolution(const SolutionBase &solution)
|
||||||
{
|
{
|
||||||
for (const auto& cb : solution_cbs_)
|
for (const auto& cb : solution_cbs_)
|
||||||
cb(solution);
|
cb(solution);
|
||||||
|
|||||||
@ -235,7 +235,7 @@ void Task::publishAllSolutions(bool wait)
|
|||||||
introspection_->publishAllSolutions(wait);
|
introspection_->publishAllSolutions(wait);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Task::onNewSolution(SolutionBase &s)
|
void Task::onNewSolution(const SolutionBase &s)
|
||||||
{
|
{
|
||||||
pimpl()->newSolution(s);
|
pimpl()->newSolution(s);
|
||||||
if (introspection_)
|
if (introspection_)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user