Merge pull request #495 from ubi-agni/debug-#485

[WIP] Debug/Fix #485

core/include/moveit/task_constructor/stage.h

@@ -184,6 +184,7 @@ class Stage
 	void setName(const std::string& name);
 
 	uint32_t introspectionId() const;
+	Introspection* introspection() const;
 
 	/** set computation timeout (in seconds)
 	 *

core/include/moveit/task_constructor/stage_p.h

@@ -301,10 +301,20 @@ class MonitoringGeneratorPrivate : public GeneratorPrivate
 };
 PIMPL_FUNCTIONS(MonitoringGenerator)
 
+// Print pending pairs of a ConnectingPrivate instance
+class ConnectingPrivate;
+struct PendingPairsPrinter
+{
+	const ConnectingPrivate* const instance_;
+	PendingPairsPrinter(const ConnectingPrivate* c) : instance_(c) {}
+};
+std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& printer);
+
 class ConnectingPrivate : public ComputeBasePrivate
 {
 	friend class Connecting;
 	friend struct FallbacksPrivateConnect;
+	friend std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& printer);
 
 public:
 	struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
@@ -337,7 +347,7 @@ class ConnectingPrivate : public ComputeBasePrivate
 	template <Interface::Direction dir>
 	bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const;
 
-	std::ostream& printPendingPairs(std::ostream& os = std::cerr) const;
+	PendingPairsPrinter pendingPairsPrinter() const { return PendingPairsPrinter(this); }
 
 private:
 	// Create a pair of Interface states for pending list, such that the order (start, end) is maintained
@@ -352,5 +362,6 @@ class ConnectingPrivate : public ComputeBasePrivate
 	ordered<StatePair> pending;
 };
 PIMPL_FUNCTIONS(Connecting)
+
 }  // namespace task_constructor
 }  // namespace moveit

core/src/container.cpp

@@ -66,7 +66,7 @@ printChildrenInterfaces(const ContainerBasePrivate& container, bool success, con
 	if (success)
 		os << ++id << ' ';
 	if (const auto conn = dynamic_cast<const ConnectingPrivate*>(creator.pimpl()))
-		conn->printPendingPairs(os);
+		os << conn->pendingPairsPrinter();
 	os << std::endl;
 
 	for (const auto& child : container.children()) {
@@ -487,7 +487,8 @@ struct SolutionCollector
 			solutions.emplace_back(std::make_pair(trace, prio));
 		} else {
 			for (SolutionBase* successor : next) {
-				assert(!successor->isFailure());  // We shouldn't have invalid solutions
+				if (successor->isFailure())
+					continue;  // skip failures
 				trace.push_back(successor);
 				traverse(*successor, prio + InterfaceState::Priority(1, successor->cost()));
 				trace.pop_back();

core/src/introspection.cpp

@@ -55,6 +55,8 @@ bool isValidCharInName(char c);  // unfortunately this is not declared in ros/na
 }  // namespace names
 }  // namespace ros
 
+static const char* LOGGER = "introspection";
+
 namespace moveit {
 namespace task_constructor {
 
@@ -206,6 +208,9 @@ uint32_t Introspection::stageId(const Stage* const s) const {
 
 uint32_t Introspection::solutionId(const SolutionBase& s) {
 	auto result = impl->id_solution_bimap_.left.insert(std::make_pair(1 + impl->id_solution_bimap_.size(), &s));
+	if (result.second)  // new entry
+		ROS_DEBUG_STREAM_NAMED(LOGGER, "new solution #" << result.first->first << " (" << s.creator()->name()
+		                                                << "): " << s.cost() << " " << s.comment());
 	return result.first->first;
 }
 

core/src/stage.cpp

@@ -381,6 +381,9 @@ uint32_t Stage::introspectionId() const {
 		throw std::runtime_error("Task is not initialized yet or Introspection was disabled.");
 	return const_cast<const moveit::task_constructor::Introspection*>(pimpl_->introspection_)->stageId(this);
 }
+Introspection* Stage::introspection() const {
+	return pimpl_->introspection_;
+}
 
 void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) {
 	const PropertyMap& src = source.properties();
@@ -791,20 +794,29 @@ void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags
 		assert(it->priority().enabled());  // new solutions are feasible, aren't they?
 		InterfacePtr other_interface = pullInterface<dir>();
 		bool have_enabled_opposites = false;
+
+		// other interface states to re-enable (post-poned because otherwise order in other_interface changes during loop)
+		std::vector<Interface::iterator> oit_to_enable;
 		for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
 			if (!static_cast<Connecting*>(me_)->compatible(*it, *oit))
 				continue;
 
 			// re-enable the opposing state oit (and its associated solution branch) if its status is ARMED
 			// https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202
-			if (oit->priority().status() == InterfaceState::Status::ARMED)
-				parent_pimpl->setStatus<opposite<dir>()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED);
+			if (oit->priority().status() == InterfaceState::Status::ARMED) {
+				oit_to_enable.push_back(oit);
+				have_enabled_opposites = true;
+			}
 			if (oit->priority().enabled())
 				have_enabled_opposites = true;
 
 			// Remember all pending states, regardless of their status!
 			pending.insert(make_pair<dir>(it, oit));
 		}
+		// actually re-enable other interface states, which were scheduled for re-enabling above
+		for (Interface::iterator oit : oit_to_enable)
+			parent_pimpl->setStatus<opposite<dir>()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED);
+
 		if (!have_enabled_opposites)  // prune new state and associated branch if necessary
 			// pass creator=nullptr to skip hasPendingOpposites() check as we did this here already
 			parent_pimpl->setStatus<dir>(nullptr, nullptr, &*it, InterfaceState::Status::ARMED);
@@ -823,7 +835,7 @@ void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags
 		os << d << " " << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl;
 	}
 	os << std::setw(15) << " ";
-	printPendingPairs(os) << std::endl;
+	os << pendingPairsPrinter() << std::endl;
 #endif
 }
 
@@ -853,6 +865,7 @@ template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const
                                                                           const InterfaceState* start) const;
 
 bool ConnectingPrivate::canCompute() const {
+	// ROS_DEBUG_STREAM("canCompute " << name() << ": " << pendingPairsPrinter());
 	// Do we still have feasible pending state pairs?
 	return !pending.empty() && pending.front().first->priority().enabled() &&
 	       pending.front().second->priority().enabled();
@@ -866,15 +879,16 @@ void ConnectingPrivate::compute() {
 	static_cast<Connecting*>(me_)->compute(from, to);
 }
 
-std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
+std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& p) {
+	const auto* impl = p.instance_;
 	const char* reset = InterfaceState::colorForStatus(3);
-	for (const auto& candidate : pending) {
-		size_t first = getIndex(*starts(), candidate.first);
-		size_t second = getIndex(*ends(), candidate.second);
+	for (const auto& candidate : impl->pending) {
+		size_t first = getIndex(*impl->starts(), candidate.first);
+		size_t second = getIndex(*impl->ends(), candidate.second);
 		os << InterfaceState::colorForStatus(candidate.first->priority().status()) << first << reset << ":"
 		   << InterfaceState::colorForStatus(candidate.second->priority().status()) << second << reset << " ";
 	}
-	if (pending.empty())
+	if (impl->pending.empty())
 		os << "---";
 	return os;
 }

core/test/CMakeLists.txt

@@ -34,9 +34,10 @@ if (CATKIN_ENABLE_TESTING)
 		mtc_add_test("gmock" ${ARGN})
 	endmacro()
 
+	mtc_add_gmock(test_mockups.cpp)
 	mtc_add_gtest(test_stage.cpp)
 	mtc_add_gtest(test_container.cpp)
-	mtc_add_gtest(test_serial.cpp)
+	mtc_add_gmock(test_serial.cpp)
 	mtc_add_gtest(test_pruning.cpp)
 	mtc_add_gtest(test_properties.cpp)
 	mtc_add_gtest(test_cost_terms.cpp)

core/test/stage_mockups.cpp

@@ -46,6 +46,19 @@ double PredefinedCosts::cost() const {
 	return c;
 }
 
+void DelayingWrapper::compute() {
+	if (!delay_.empty()) {  // if empty, we don't delay
+		if (delay_.front() == 0)
+			delay_.pop_front();  // we can compute() now
+		else {
+			--delay_.front();  // continue delaying
+			return;
+		}
+	}
+	// forward to child, eventually generating multiple solutions at once
+	WrapperBase::compute();
+}
+
 GeneratorMockup::GeneratorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
   : Generator{ "GEN" + std::to_string(++id_) }
   , costs_{ std::move(costs) }
@@ -86,6 +99,7 @@ void ConnectMockup::compute(const InterfaceState& from, const InterfaceState& to
 
 	auto solution{ std::make_shared<SubTrajectory>() };
 	solution->setCost(costs_.cost());
+	solution->setComment(std::to_string(from.priority().cost()) + " -> " + std::to_string(to.priority().cost()));
 	connect(from, to, solution);
 }
 

core/test/stage_mockups.h

@@ -34,6 +34,18 @@ struct PredefinedCosts : CostTerm
 
 constexpr double INF{ std::numeric_limits<double>::infinity() };
 
+/* wrapper stage to delay solutions by a given number of steps */
+struct DelayingWrapper : public WrapperBase
+{
+	std::list<unsigned int> delay_;
+	/* delay list specifies the number of steps each received solution should be delayed */
+	DelayingWrapper(std::list<unsigned int> delay, Stage::pointer&& child)
+	  : WrapperBase("delayer", std::move(child)), delay_{ std::move(delay) } {}
+
+	void compute() override;
+	void onNewSolution(const SolutionBase& s) override { liftSolution(s); }
+};
+
 struct GeneratorMockup : public Generator
 {
 	planning_scene::PlanningScenePtr ps_;
@@ -47,8 +59,8 @@ struct GeneratorMockup : public Generator
 	// default to one solution to avoid infinity loops
 	GeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ std::list<double>{ 0.0 }, true },
 	                std::size_t solutions_per_compute = 1);
-	GeneratorMockup(std::initializer_list<double> costs)
-	  : GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
+	GeneratorMockup(std::initializer_list<double> costs, std::size_t solutions_per_compute = 1)
+	  : GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true }, solutions_per_compute } {}
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 	bool canCompute() const override;

core/test/test_container.cpp

@@ -110,7 +110,7 @@ class InitTest : public ::testing::Test
 	Container container;
 	InterfacePtr dummy;
 
-	InitTest() : ::testing::Test{}, robot_model{ getModel() }, dummy{ new Interface } {}
+	InitTest() : ::testing::Test{}, robot_model{ getModel() }, dummy{ std::make_shared<Interface>() } {}
 
 	void pushInterface(bool start = true, bool end = true) {
 		// pretend, that the container is connected

core/test/test_mockups.cpp

@@ -0,0 +1,76 @@
+#include "stage_mockups.h"
+#include <moveit/task_constructor/container_p.h>
+
+using namespace moveit::task_constructor;
+
+struct TestGeneratorMockup : public GeneratorMockup
+{
+	InterfacePtr next_starts_;
+	InterfacePtr prev_ends_;
+
+	using GeneratorMockup::GeneratorMockup;
+	using GeneratorMockup::init;
+
+	void init() {
+		next_starts_ = std::make_shared<Interface>();
+		prev_ends_ = std::make_shared<Interface>();
+
+		GeneratorMockup::reset();
+		GeneratorMockup::init(getModel());
+
+		GeneratorPrivate* impl = pimpl();
+		impl->setNextStarts(next_starts_);
+		impl->setPrevEnds(prev_ends_);
+	}
+};
+
+TEST(GeneratorMockup, compute) {
+	TestGeneratorMockup gen({ 1.0, 2.0, 3.0 });
+	gen.init();
+
+	for (int i = 0; i < 3; ++i) {
+		ASSERT_TRUE(gen.canCompute());
+		gen.compute();
+	}
+	EXPECT_FALSE(gen.canCompute());
+
+	EXPECT_COSTS(gen.solutions(), ::testing::ElementsAre(1, 2, 3));
+}
+
+#define COMPUTE_EXPECT_COSTS(stage, ...)                                                 \
+	{                                                                                     \
+		EXPECT_TRUE(stage.canCompute());                                                   \
+		stage.compute();                                                                   \
+		EXPECT_COSTS(stage.solutions(), ::testing::ElementsAre(__VA_ARGS__));              \
+		constexpr auto num_elements = std::initializer_list<double>{ __VA_ARGS__ }.size(); \
+		EXPECT_EQ(runs, num_elements);                                                     \
+	}
+
+TEST(GeneratorMockup, delayed) {
+	auto gen = std::make_unique<TestGeneratorMockup>(PredefinedCosts({ 1.0, 2.0, 3.0 }));
+	gen->init();
+	auto& runs = gen->runs_;
+
+	DelayingWrapper w({ 1, 0, 2 }, std::move(gen));
+	auto prev_ends = std::make_shared<Interface>();
+	auto next_starts = std::make_shared<Interface>();
+
+	WrapperBasePrivate* impl = w.pimpl();
+	impl->setPrevEnds(prev_ends);
+	impl->setNextStarts(next_starts);
+
+	// 1st compute() is delayed by 1
+	COMPUTE_EXPECT_COSTS(w);
+	COMPUTE_EXPECT_COSTS(w, 1);
+
+	// 2nd compute() is not delayed
+	COMPUTE_EXPECT_COSTS(w, 1, 2);
+
+	// 1st compute() is delayed by 2
+	COMPUTE_EXPECT_COSTS(w, 1, 2);
+	COMPUTE_EXPECT_COSTS(w, 1, 2);
+	COMPUTE_EXPECT_COSTS(w, 1, 2, 3);
+
+	EXPECT_FALSE(w.canCompute());
+}
+#undef COMPUTE_EXPECT_COSTS

core/test/test_pruning.cpp

@@ -6,8 +6,6 @@
 #include <list>
 #include <memory>
 
-#include <gtest/gtest.h>
-
 #ifndef TYPED_TEST_SUITE
 #define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES)
 #endif
@@ -100,14 +98,7 @@ TEST_F(Pruning, ConnectConnectForward) {
 	add(t, new GeneratorMockup({ 1, 2, 3 }));
 
 	t.plan();
-
-	ASSERT_EQ(t.solutions().size(), 3u * 2u);
-	std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
-	auto expected_cost = expected_costs.begin();
-	for (const auto& s : t.solutions()) {
-		EXPECT_EQ(s->cost(), *expected_cost);
-		++expected_cost;
-	}
+	EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(11, 12, 13, 21, 22, 23));
 	EXPECT_EQ(c1->runs_, 3u);
 	EXPECT_EQ(c2->runs_, 6u);  // expect 6 instead of 9 calls
 }
@@ -123,13 +114,7 @@ TEST_F(Pruning, ConnectConnectBackward) {
 
 	t.plan();
 
-	ASSERT_EQ(t.solutions().size(), 3u * 2u);
-	std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
-	auto expected_cost = expected_costs.begin();
-	for (const auto& s : t.solutions()) {
-		EXPECT_EQ(s->cost(), *expected_cost);
-		++expected_cost;
-	}
+	EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(11, 12, 13, 21, 22, 23));
 	EXPECT_EQ(c1->runs_, 6u);  // expect 6 instead of 9 calls
 	EXPECT_EQ(c2->runs_, 3u);
 }
@@ -207,3 +192,18 @@ TEST_F(Pruning, TwoConnects) {
 
 	EXPECT_FALSE(t.plan());
 }
+
+TEST_F(Pruning, BackPropagateFailure) {
+	add(t, new GeneratorMockup({ 1.0 }));
+	auto con1 = add(t, new ConnectMockup());
+	add(t, new GeneratorMockup({ 10.0, 20.0 }, 2));  // create all solutions on first run
+	auto con2 = add(t, new ConnectMockup());
+	add(t, new GeneratorMockup({ 100.0, 200.0 }, 2));  // create all solutions on first run
+	// delay failure (INF) until CON2 has found first solution
+	add(t, new DelayingWrapper({ 1 }, std::make_unique<ForwardMockup>(PredefinedCosts({ INF, 2000 }))));
+
+	EXPECT_TRUE(t.plan());
+	EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(2211, 2221));
+	EXPECT_EQ(con1->runs_, 2u);
+	EXPECT_EQ(con2->runs_, 3u);  // 100 - 20 is pruned
+}