From f4fb8c6f7188813f2767b41c44cd9fd2e0fa093b Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 5 Mar 2021 14:34:19 +0100 Subject: [PATCH] fix computeCost interface (again) Fixes the missing creator pointer introduced in a6a23d3775be81cdd5920d8c79e218edc199f7c5 --- .../include/moveit/task_constructor/storage.h | 4 +-- core/src/stage.cpp | 26 ++++++++++--------- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index fd709e4a..6896e2a6 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -197,12 +197,12 @@ private: class CostTerm; class StagePrivate; class ContainerBasePrivate; -struct TmpInterfaceStateProvider; +struct TmpSolutionContext; /// abstract base class for solutions (primitive and sequences) class SolutionBase { friend ContainerBasePrivate; - friend TmpInterfaceStateProvider; + friend TmpSolutionContext; public: virtual ~SolutionBase() = default; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index a7337710..d1b141b3 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -234,19 +234,21 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { // To solve the chicken-egg problem in computeCost() and provide proper states at both ends of the solution, // this class temporarily sets the new interface states w/o registering the solution yet. // On destruction the start/end states are reset again. -struct TmpInterfaceStateProvider +struct TmpSolutionContext { - SolutionBase* solution_; - TmpInterfaceStateProvider(SolutionBase& solution, const InterfaceState& from, const InterfaceState& to) - : solution_(&solution) { - assert(solution_->start_ == nullptr); - assert(solution_->end_ == nullptr); - solution_->start_ = &from; - solution_->end_ = &to; + SolutionBase& solution_; + TmpSolutionContext(SolutionBase& solution, Stage* creator, const InterfaceState& from, const InterfaceState& to) + : solution_(solution) { + assert(solution_.start_ == nullptr); + assert(solution_.end_ == nullptr); + solution_.start_ = &from; + solution_.end_ = &to; + solution_.creator_ = creator; } - ~TmpInterfaceStateProvider() { - solution_->start_ = nullptr; - solution_->end_ = nullptr; + ~TmpSolutionContext() { + solution_.start_ = nullptr; + solution_.end_ = nullptr; + solution_.creator_ = nullptr; } }; void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) { @@ -256,7 +258,7 @@ void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& // Temporarily set start/end states of the solution w/o actually registering the solution with them // This allows CostTerms to compute costs based on the InterfaceState. - TmpInterfaceStateProvider tip(solution, from, to); + TmpSolutionContext tip(solution, me(), from, to); std::string comment; assert(cost_term_);