add modifier with both modified start/end states

Required to lift a SolutionSequence, and modify its states (e.g., to add properties).

Author: v4hn

core/include/moveit/task_constructor/container.h

@@ -131,6 +131,10 @@ class ParallelContainerBase : public ContainerBase
 	/// lift a modified solution, changing the (single!) new associated start or end InterfaceState
 	void liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_propagated_state,
 	                          const SolutionBase& child_solution);
+	/// lift a modified solution, providing new start and end states
+	/// The new states will only be used if this's should actually create the corresponding states
+	void liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_start_state,
+	                          InterfaceState&& new_end_state, const SolutionBase& child_solution);
 };
 
 /** Plan for different alternatives in parallel.

core/include/moveit/task_constructor/container_p.h

@@ -150,9 +150,11 @@ class ContainerBasePrivate : public StagePrivate
 	/// copy external_state to a child's interface and remember the link in internal_external map
 	template <Interface::Direction>
 	void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
-	/// lift solution from internal to external level, possibly replacing the generated InterfaceState with new_external
+	/// lift solution from internal to external level, possibly replacing the generated InterfaceStates with new_*
+	/// If specified, *new_from/*new_to will be moved from.
 	void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
-	                  const InterfaceState* internal_to, const InterfaceState* new_external = nullptr);
+	                  const InterfaceState* internal_to, InterfaceState* new_from = nullptr,
+	                  InterfaceState* new_to = nullptr);
 
 	/// protected writable overloads
 	inline auto& internalToExternalMap() { return internal_external_.left; }

core/src/container.cpp

@@ -203,13 +203,14 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
 }
 
 void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
-                                        const InterfaceState* internal_to, const InterfaceState* new_external) {
+                                        const InterfaceState* internal_to, InterfaceState* new_from,
+                                        InterfaceState* new_to) {
 	const bool create_from{ requiredInterface().testFlag(WRITES_PREV_END) };
 	const bool create_to{ requiredInterface().testFlag(WRITES_NEXT_START) };
 
 	// external states, nullptr if they don't exist yet
-	const InterfaceState* external_from{ create_from ? new_external : internalToExternalMap().at(internal_from) };
-	const InterfaceState* external_to{ create_to ? new_external : internalToExternalMap().at(internal_to) };
+	const InterfaceState* external_from{ create_from ? new_from : internalToExternalMap().at(internal_from) };
+	const InterfaceState* external_to{ create_to ? new_to : internalToExternalMap().at(internal_to) };
 
 	// computeCost
 	// we can pass intern_{from/to} here because in this case the lifted states that might be created later
@@ -221,17 +222,16 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
 		return;
 	}
 
-	auto create_state = [this, new_external](const InterfaceState& internal) {
-		InterfaceState* external{ storeState(new_external ? InterfaceState{ *new_external } :
-			                                                 InterfaceState{ internal }) };
+	auto create_state = [this](const InterfaceState& internal, InterfaceState* new_external) {
+		InterfaceState* external{ storeState(new_external ? std::move(*new_external) : InterfaceState{ internal }) };
 		internalToExternalMap().insert(std::make_pair(&internal, external));
 		return external;
 	};
 
 	if (create_from)
-		external_from = create_state(*internal_from);
+		external_from = create_state(*internal_from, new_from);
 	if (create_to)
-		external_to = create_state(*internal_to);
+		external_to = create_state(*internal_to, new_to);
 
 	assert(external_from);
 	assert(external_to);
@@ -749,7 +749,7 @@ void ParallelContainerBase::liftSolution(const SolutionBase& solution, double co
 	                      solution.start(), solution.end());
 }
 
-void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& modified_solution, const SolutionBase &child_solution) {
+void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& modified_solution, const SolutionBase& child_solution) {
 	// child_solution is correctly prepared by a child of this container
 	assert(child_solution.creator());
 	assert(child_solution.creator()->parent() == this);
@@ -758,13 +758,23 @@ void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& modified_solu
 	                      child_solution.start(), child_solution.end());
 }
 
-void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr &&new_solution, InterfaceState &&new_propagated_state, const SolutionBase &child_solution) {
+void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_propagated_state, const SolutionBase& child_solution) {
 	assert(child_solution.creator());
 	assert(child_solution.creator()->parent() == this);
+	assert(pimpl()->requiredInterface() == PROPAGATE_FORWARDS || pimpl()->requiredInterface() == PROPAGATE_BACKWARDS);
 
-	pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_propagated_state);
+	pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_propagated_state, &new_propagated_state);
 }
 
+void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_from, InterfaceState&& new_to, const SolutionBase& child_solution) {
+	assert(child_solution.creator());
+	assert(child_solution.creator()->parent() == this);
+	assert(pimpl()->requiredInterface() == PROPAGATE_FORWARDS || pimpl()->requiredInterface() == PROPAGATE_BACKWARDS);
+
+	pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_from, &new_to);
+}
+
+
 WrapperBasePrivate::WrapperBasePrivate(WrapperBase* me, const std::string& name)
   : ParallelContainerBasePrivate(me, name) {}