introduce abstraction storeState

analogous to storeSolution

Author: v4hn

core/include/moveit/task_constructor/stage_p.h

@@ -144,7 +144,10 @@ class StagePrivate
 	void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
 	void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
 
+	// store stage-owned data structures
 	bool storeSolution(const SolutionBasePtr& solution, const InterfaceState* from, const InterfaceState* to);
+	InterfaceState* storeState(InterfaceState&& state);
+
 	void newSolution(const SolutionBasePtr& solution);
 	bool storeFailures() const { return introspection_ != nullptr; }
 	void runCompute() {

core/src/container.cpp

@@ -212,7 +212,7 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
 		if (it != internalToExternalMap().end())
 			return const_cast<InterfaceState*>(it->second);
 
-		InterfaceState* external = &*states_.insert(states_.end(), InterfaceState(*internal));
+		InterfaceState* external = storeState(InterfaceState(*internal));
 		internalToExternalMap().insert(std::make_pair(internal, external));
 		created = true;
 		return external;

core/src/stage.cpp

@@ -158,14 +158,14 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to,
 
 	me()->forwardProperties(from, to);
 
-	auto to_it = states_.insert(states_.end(), std::move(to));
+	InterfaceState* to_stored{ storeState(std::move(to)) };
 
 	// register stored interfaces with solution
 	solution->setStartState(from);
-	solution->setEndState(*to_it);
+	solution->setEndState(*to_stored);
 
 	if (!solution->isFailure())
-		nextStarts()->add(*to_it);
+		nextStarts()->add(*to_stored);
 
 	newSolution(solution);
 }
@@ -180,13 +180,13 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
 
 	me()->forwardProperties(to, from);
 
-	auto from_it = states_.insert(states_.end(), std::move(from));
+	InterfaceState* from_stored{ storeState(std::move(from)) };
 
-	solution->setStartState(*from_it);
+	solution->setStartState(*from_stored);
 	solution->setEndState(to);
 
 	if (!solution->isFailure())
-		prevEnds()->add(*from_it);
+		prevEnds()->add(*from_stored);
 
 	newSolution(solution);
 }
@@ -199,8 +199,8 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
 	if (!storeSolution(solution, nullptr, nullptr))
 		return;  // solution dropped
 
-	auto from = states_.insert(states_.end(), InterfaceState(state));  // copy
-	auto to = states_.insert(states_.end(), std::move(state));
+	InterfaceState* from{ storeState(InterfaceState{ state }) };
+	InterfaceState* to{ storeState(std::move(state)) };
 
 	solution->setStartState(*from);
 	solution->setEndState(*to);
@@ -275,6 +275,10 @@ void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState&
 	}
 }
 
+InterfaceState* StagePrivate::storeState(InterfaceState&& state) {
+	return &*states_.insert(states_.end(), std::move(state));
+}
+
 Stage::Stage(StagePrivate* impl) : pimpl_(impl) {
 	assert(impl);
 	auto& p = properties();