FallbacksPrivateConnect

Implement Fallbacks behavior for children of type Connecting.
All other connect-like children are currently infeasible to handle,
because we cannot forward a single job, i.e. a pair (from, to)
to the next child, but only individual states.
However, passing states, will cause creation of undesired state pairs
as jobs in subsequent children.

Author: rhaschke

core/include/moveit/task_constructor/container_p.h

@@ -302,6 +302,21 @@ struct FallbacksPrivatePropagator : FallbacksPrivateCommon
 	bool job_has_solutions_;  // flag indicating whether the current job generated solutions
 };
 
+/// Fallbacks implementation for CONNECT interface
+struct FallbacksPrivateConnect : FallbacksPrivate
+{
+	FallbacksPrivateConnect(FallbacksPrivate&& old);
+	void reset() override;
+	bool canCompute() const override;
+	void compute() override;
+	void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
+
+	template <Interface::Direction dir>
+	void propagateStateUpdate(Interface::iterator external, bool updated);
+
+	mutable container_type::const_iterator active_;  // child picked for compute()
+};
+
 class WrapperBasePrivate : public ParallelContainerBasePrivate
 {
 	friend class WrapperBase;

core/include/moveit/task_constructor/stage_p.h

@@ -301,6 +301,7 @@ PIMPL_FUNCTIONS(MonitoringGenerator)
 class ConnectingPrivate : public ComputeBasePrivate
 {
 	friend class Connecting;
+	friend struct FallbacksPrivateConnect;
 
 public:
 	struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>

core/src/container.cpp

@@ -876,7 +876,11 @@ inline void Fallbacks::replaceImpl() {
 			impl = new FallbacksPrivatePropagator(std::move(*impl));
 			break;
 		case CONNECT:
-			throw std::runtime_error("Not yet implemented");
+			// For now, we only support Connecting children
+			for (const auto& child : impl->children())
+				if (!dynamic_cast<Connecting*>(child.get()))
+					throw std::runtime_error("CONNECT-like interface is only supported for Connecting children");
+			impl = new FallbacksPrivateConnect(std::move(*impl));
 			break;
 	}
 	delete pimpl_;
@@ -1017,6 +1021,65 @@ bool FallbacksPrivatePropagator::nextJob() {
 }
 
 
+FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old)
+	: FallbacksPrivate(std::move(old)) {
+	starts_ = std::make_shared<Interface>(
+		std::bind(&FallbacksPrivateConnect::propagateStateUpdate<Interface::FORWARD>, this, std::placeholders::_1, std::placeholders::_2));
+	ends_ = std::make_shared<Interface>(
+		std::bind(&FallbacksPrivateConnect::propagateStateUpdate<Interface::BACKWARD>, this, std::placeholders::_1, std::placeholders::_2));
+
+	FallbacksPrivateConnect::reset();
+}
+
+void FallbacksPrivateConnect::reset() {
+	active_ = children().end();
+}
+
+template <Interface::Direction dir>
+void FallbacksPrivateConnect::propagateStateUpdate(Interface::iterator external, bool updated) {
+	copyState<dir>(external, children().front()->pimpl()->pullInterface(dir), updated);
+	// TODO: propagate updates to other children as well
+}
+
+bool FallbacksPrivateConnect::canCompute() const {
+	for (auto it=children().begin(), end=children().end(); it!=end; ++it)
+		if ((*it)->pimpl()->canCompute()) {
+			active_ = it;
+			return true;
+		}
+	active_ = children().end();
+	return false;
+}
+
+void FallbacksPrivateConnect::compute() {
+	// Alternatively, we could also compute() all children that canCompute()
+	assert(active_ != children().end());
+	(*active_)->pimpl()->runCompute();
+}
+
+void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
+	// expect failure to be reported from active child
+	assert(active_ != children().end() && active_->get() == &child);
+	// ... thus we can use std::next(active_) to find the next child
+	auto next = std::next(active_);
+
+	auto findIteratorFor = [](const InterfaceState* state, const Interface& interface) {
+		auto it = std::find(interface.begin(), interface.end(), state);
+		assert(it != interface.end());
+		return it;
+	};
+
+	if (next != children().end()) {  // pass job to next child
+		auto next_con = static_cast<ConnectingPrivate*>(const_cast<StagePrivate*>((*next)->pimpl()));
+		auto first_con = static_cast<const ConnectingPrivate*>(children().front()->pimpl());
+		auto fromIt = findIteratorFor(from, *first_con->starts());
+		auto toIt = findIteratorFor(to, *first_con->ends());
+		next_con->pending.insert(std::make_pair(fromIt, toIt));
+	} else  // or report failure to parent
+		parent()->pimpl()->onNewFailure(*me(), from, to);
+}
+
+
 MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
 
 void MergerPrivate::resolveInterface(InterfaceFlags expected) {