diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index a627d8fb..67027150 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -346,10 +346,8 @@ public: void reset() override; virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0; - void connect(const InterfaceState& from, const InterfaceState& to, - SubTrajectory&& trajectory); - void connect(const InterfaceState& from, const InterfaceState& to, - SubTrajectory&& trajectory, double cost) { + void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory); + void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory, double cost) { trajectory.setCost(cost); connect(from, to, std::move(trajectory)); } diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index ecc5eb43..90320699 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -103,7 +103,7 @@ public: inline void setIntrospection(Introspection* introspection) { introspection_ = introspection; } void composePropertyErrorMsg(const std::string& name, std::ostream& os); - void newSolution(const SolutionBase ¤t); + void newSolution(SolutionBase& current); protected: Stage* const me_; // associated/owning Stage instance diff --git a/core/src/container.cpp b/core/src/container.cpp index 7b12e37e..f8cc800f 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -856,7 +856,6 @@ void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t) auto impl = pimpl(); assert(impl->prevEnds() && impl->nextStarts()); - t.setCreator(impl); // store newly created solution (otherwise it's lost) auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t)); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 67260434..11ace2c7 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -83,8 +83,10 @@ InterfaceFlags StagePrivate::interfaceFlags() const return f; } -void StagePrivate::newSolution(const SolutionBase &solution) +void StagePrivate::newSolution(SolutionBase &solution) { + solution.setCreator(this); + if (introspection_) introspection_->registerSolution(solution); @@ -249,7 +251,6 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { } SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) { - trajectory.setCreator(this); if (!trajectory.isFailure()) { return *solutions_.insert(std::move(trajectory)); } else if (me()->storeFailures()) { @@ -653,8 +654,7 @@ void Connecting::reset() ComputeBase::reset(); } -void Connecting::connect(const InterfaceState& from, const InterfaceState& to, - SubTrajectory&& t) { +void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) { auto impl = pimpl(); SubTrajectory& trajectory = impl->addTrajectory(std::move(t)); trajectory.setStartState(from);