support modifying wrappers in user stages

core/include/moveit/task_constructor/container.h

@@ -127,12 +127,15 @@ class ParallelContainerBase : public ContainerBase
 	/// lift child solution to external interface, adapting the costs and comment
 	void liftSolution(const SolutionBase& solution, double cost, std::string comment);
 
-	/// spawn a new solution with given state as start and end
-	void spawn(InterfaceState&& state, SubTrajectory&& trajectory);
-	/// propagate a solution forwards
-	void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory);
-	/// propagate a solution backwards
-	void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& trajectory);
+	/// lift a modified solution based on the solution of a child stage
+	void liftModifiedSolution(const SolutionBasePtr& new_solution, const SolutionBase& child_solution);
+	/// lift a modified solution, changing the (single!) new associated start or end InterfaceState
+	void liftModifiedSolution(const 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(const 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
+	/// 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* internal_to, InterfaceState* new_from = nullptr,
+	                  InterfaceState* new_to = nullptr);
 
 	/// protected writable overloads
 	inline auto& internalToExternalMap() { return internal_external_.left; }

core/include/moveit/task_constructor/stage_p.h

@@ -146,7 +146,12 @@ 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);
+	inline InterfaceState* storeState(InterfaceState&& state) {
+		return &*states_.insert(states_.end(), std::move(state));
+	}
+
 	void newSolution(const SolutionBasePtr& solution);
 	bool storeFailures() const { return introspection_ != nullptr; }
 	void runCompute() {

core/src/container.cpp

@@ -196,44 +196,99 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
 	}
 
 	// create a clone of external state within target interface (child's starts() or ends())
-	auto internal = states_.insert(states_.end(), InterfaceState(*external));
-	target->add(*internal);
+	InterfaceState* internal = storeState(InterfaceState{ *external });
+
 	// and remember the mapping between them
-	internalToExternalMap().insert(std::make_pair(&*internal, &*external));
+	internalToExternalMap().insert(std::make_pair(internal, &*external));
+	target->add(*internal);
 }
 
 void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
-                                        const InterfaceState* internal_to) {
-	computeCost(*internal_from, *internal_to, *solution);
-
-	// map internal to external states
-	auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* {
+                                        const InterfaceState* internal_to, InterfaceState* new_from,
+                                        InterfaceState* new_to) {
+	// NOLINTNEXTLINE(readability-identifier-naming)
+	auto findExternal = [this](const InterfaceState* internal) -> const InterfaceState* {
 		auto it = internalToExternalMap().find(internal);
-		if (it != internalToExternalMap().end())
-			return const_cast<InterfaceState*>(it->second);
+		if (it != internalToExternalMap().end()) {
+			return &*it->second;
+		}
 
-		InterfaceState* external = &*states_.insert(states_.end(), InterfaceState(*internal));
-		internalToExternalMap().insert(std::make_pair(internal, external));
-		created = true;
-		return external;
+		return nullptr;
 	};
-	bool created_from = false;
-	bool created_to = false;
-	InterfaceState* external_from = find_or_create_external(internal_from, created_from);
-	InterfaceState* external_to = find_or_create_external(internal_to, created_to);
 
-	if (!storeSolution(solution, external_from, external_to))
+	// external states, nullptr if they don't exist yet
+	const InterfaceState* external_from{ findExternal(internal_from) };
+	const InterfaceState* external_to{ findExternal(internal_to) };
+
+	// TODO(v4hn) rethink this. ComputeIK is exactly this case. Do I want to support an n:m mapping from internal to
+	// external?
+	if ((new_from && external_from && external_from != new_from) || (new_to && external_to && external_to != new_to)) {
+		ROS_ERROR_STREAM_NAMED("Container", "Container '" << name_ << "' tried to lift a modified solution from child '"
+		                                                  << solution->creator()->name()
+		                                                  << "', but a different one already exists. Children's "
+		                                                     "InterfaceStates can only ever match one ");
 		return;
+	}
+
+	// computeCost
+	// If there are no external states known yet, we can pass internal_{from/to} here
+	// because in this case the lifted states that will be created later
+	// are equivalent up to the connected Solutions (which are not relevant for CostTerms)
+	{
+		auto getPreliminaryState = [](const InterfaceState* external, const InterfaceState* new_state,
+		                              const InterfaceState* internal) -> const InterfaceState& {
+			if (external)
+				return *external;
+			if (new_state)
+				return *new_state;
+			else
+				return *internal;
+		};
+		computeCost(getPreliminaryState(external_from, new_from, internal_from),
+		            getPreliminaryState(external_to, new_to, internal_to), *solution);
+	}
+
+	// storeSolution
+	// only pass stored external states here (others are not relevant for pruning)
+	if (!storeSolution(solution, external_from, external_to)) {
+		return;
+	}
+
+	// store unstored states in stage-internal storage
+
+	// NOLINTNEXTLINE(readability-identifier-naming)
+	auto createState = [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;
+	};
+
+	const bool create_from{ external_from == nullptr };
+	const bool create_to{ external_to == nullptr };
+
+	if (create_from) {
+		assert(requiredInterface().testFlag(WRITES_PREV_END));
+		external_from = createState(*internal_from, new_from);
+	}
+	if (create_to) {
+		assert(requiredInterface().testFlag(WRITES_NEXT_START));
+		external_to = createState(*internal_to, new_to);
+	}
 
-	// connect solution to start/end state
+	assert(external_from);
+	assert(external_to);
+
+	// connect solution to stored states
 	solution->setStartState(*external_from);
 	solution->setEndState(*external_to);
 
-	// spawn created states in external interfaces
-	if (created_from)
-		prevEnds()->add(*external_from);
-	if (created_to)
-		nextStarts()->add(*external_to);
+	// spawn new external states
+	if (!solution->isFailure()) {
+		if (create_from)
+			prevEnds()->add(*const_cast<InterfaceState*>(external_from));
+		if (create_to)
+			nextStarts()->add(*const_cast<InterfaceState*>(external_to));
+	}
 
 	newSolution(solution);
 }
@@ -735,18 +790,37 @@ void ParallelContainerBase::liftSolution(const SolutionBase& solution, double co
 	                      solution.start(), solution.end());
 }
 
-void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) {
-	pimpl()->StagePrivate::spawn(std::move(state), std::make_shared<SubTrajectory>(std::move(t)));
+void ParallelContainerBase::liftModifiedSolution(const 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);
+
+	pimpl()->liftSolution(modified_solution, child_solution.start(), child_solution.end());
 }
 
-void ParallelContainerBase::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
-	pimpl()->StagePrivate::sendForward(from, std::move(to), std::make_shared<SubTrajectory>(std::move(t)));
+void ParallelContainerBase::liftModifiedSolution(const SolutionBasePtr& new_solution, InterfaceState&& new_propagated_state, const SolutionBase& child_solution) {
+	assert(child_solution.creator());
+	assert(child_solution.creator()->parent() == this);
+
+	if(pimpl()->requiredInterface() == GENERATE){
+		// in this case we need a second InterfaceState to move from
+		InterfaceState new_to{ new_propagated_state };
+		pimpl()->liftSolution(new_solution, child_solution.start(), child_solution.end(), &new_propagated_state, &new_to);
+	}
+	else {
+		// pass new_propagated_state as start *and* end. We know at most one will be used.
+		pimpl()->liftSolution(new_solution, child_solution.start(), child_solution.end(), &new_propagated_state, &new_propagated_state);
+	}
 }
 
-void ParallelContainerBase::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) {
-	pimpl()->StagePrivate::sendBackward(std::move(from), to, std::make_shared<SubTrajectory>(std::move(t)));
+void ParallelContainerBase::liftModifiedSolution(const SolutionBasePtr& new_solution, InterfaceState&& new_from, InterfaceState&& new_to, const SolutionBase& child_solution) {
+	assert(child_solution.creator());
+	assert(child_solution.creator()->parent() == this);
+
+	pimpl()->liftSolution(new_solution, child_solution.start(), child_solution.end(), &new_from, &new_to);
 }
 
+
 WrapperBasePrivate::WrapperBasePrivate(WrapperBase* me, const std::string& name)
   : ParallelContainerBasePrivate(me, name) {}
 

core/src/stage.cpp

@@ -160,14 +160,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);
 }
@@ -182,13 +182,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);
 }
@@ -201,8 +201,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);

core/src/stages/compute_ik.cpp

@@ -335,15 +335,15 @@ void ComputeIK::compute() {
 	                                     ->getParentJointModel()
 	                                     ->getDescendantLinkModels();
 	if (colliding) {
-		SubTrajectory solution;
+		auto solution = std::make_shared<SubTrajectory>();
 		generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
-		std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
-		solution.markAsFailure();
+		std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution->markers()));
+		solution->markAsFailure();
 		// TODO: visualize collisions
-		solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
+		solution->setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
 		auto colliding_scene{ scene->diff() };
 		colliding_scene->setCurrentState(sandbox_state);
-		spawn(InterfaceState(colliding_scene), std::move(solution));
+		liftModifiedSolution(solution, InterfaceState(colliding_scene), s);
 		return;
 	} else
 		generateVisualMarkers(sandbox_state, appender, links_to_visualize);
@@ -396,18 +396,18 @@ void ComputeIK::compute() {
 		for (size_t i = previous; i != ik_solutions.size(); ++i) {
 			// create a new scene for each solution as they will have different robot states
 			planning_scene::PlanningScenePtr solution_scene = scene->diff();
-			SubTrajectory solution;
-			solution.setComment(s.comment());
+			auto solution = std::make_shared<SubTrajectory>();
+			solution->setComment(s.comment());
 
 			// frames at target pose and ik frame
-			rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
-			rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
+			rviz_marker_tools::appendFrame(solution->markers(), target_pose_msg, 0.1, "ik frame");
+			rviz_marker_tools::appendFrame(solution->markers(), ik_pose_msg, 0.1, "ik frame");
 
 			if (succeeded && i + 1 == ik_solutions.size())
 				// compute cost as distance to compare_pose
-				solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
+				solution->setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
 			else  // found an IK solution, but this was not valid
-				solution.markAsFailure();
+				solution->markAsFailure();
 
 			// set scene's robot state
 			robot_state::RobotState& solution_state = solution_scene->getCurrentStateNonConst();
@@ -416,7 +416,7 @@ void ComputeIK::compute() {
 
 			InterfaceState state(solution_scene);
 			forwardProperties(*s.start(), state);
-			spawn(std::move(state), std::move(solution));
+			liftModifiedSolution(solution, std::move(state), s);
 		}
 
 		// TODO: magic constant should be a property instead ("current_seed_only", or equivalent)
@@ -428,15 +428,15 @@ void ComputeIK::compute() {
 
 	if (ik_solutions.empty()) {  // failed to find any solution
 		planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
-		SubTrajectory solution;
+		auto solution = std::make_shared<SubTrajectory>();
 
-		solution.markAsFailure();
-		solution.setComment(s.comment() + " no IK found");
+		solution->markAsFailure();
+		solution->setComment(s.comment() + " no IK found");
 
 		// ik target link placement
-		std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
+		std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution->markers()));
 
-		spawn(InterfaceState(scene), std::move(solution));
+		liftModifiedSolution(solution, InterfaceState(scene), s);
 	}
 }
 }  // namespace stages

core/test/test_cost_terms.cpp

@@ -310,7 +310,7 @@ TEST(CostTerm, CompositeSolutions) {
 	{
 		auto s1{ std::make_unique<ForwardCostMockup>() };
 		auto s2{ std::make_unique<ForwardCostMockup>() };
-		auto c1{ std::make_unique<SerialContainer>() };
+		auto c1{ std::make_unique<SerialContainer>("c1") };
 		auto constant1{ std::make_shared<cost::Constant>(1.0) };
 		s1->setCostTerm(constant1);
 		s2->setCostTerm(constant1);
@@ -334,7 +334,7 @@ TEST(CostTerm, CompositeSolutionsContainerCost) {
 	auto s1_ptr{ s1.get() };
 	auto s2{ std::make_unique<ForwardTrajectoryMockup>() };
 
-	auto c1{ std::make_unique<SerialContainer>() };
+	auto c1{ std::make_unique<SerialContainer>("c1") };
 	c1->add(std::move(s1));
 	c1->add(std::move(s2));