Stage::explainFailure() (#445)

... to facilitate spotting the stage causing a task to fail

Author: rhaschke

core/include/moveit/task_constructor/container.h

@@ -76,6 +76,7 @@ class ContainerBase : public Stage
 
 	virtual bool canCompute() const = 0;
 	virtual void compute() = 0;
+	void explainFailure(std::ostream& os) const override;
 
 	/// called by a (direct) child when a new solution becomes available
 	virtual void onNewSolution(const SolutionBase& s) = 0;

core/include/moveit/task_constructor/stage.h

@@ -232,6 +232,8 @@ class Stage
 	/// Should we generate failure solutions? Note: Always report a failure!
 	bool storeFailures() const;
 
+	virtual void explainFailure(std::ostream& os) const;
+
 	/// Get the stage's property map
 	PropertyMap& properties();
 	const PropertyMap& properties() const { return const_cast<Stage*>(this)->properties(); }

core/include/moveit/task_constructor/task.h

@@ -132,6 +132,9 @@ class Task : protected WrapperBase
 	/// print current task state (number of found solutions and propagated states) to std::cout
 	void printState(std::ostream& os = std::cout) const;
 
+	/// print an explanation for a planning failure to os
+	void explainFailure(std::ostream& os = std::cout) const override;
+
 	size_t numSolutions() const { return solutions().size(); }
 	const ordered<SolutionBaseConstPtr>& solutions() const { return stages()->solutions(); }
 	const std::list<SolutionBaseConstPtr>& failures() const { return stages()->failures(); }

core/src/container.cpp

@@ -446,6 +446,20 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
 		throw errors;
 }
 
+void ContainerBase::explainFailure(std::ostream& os) const {
+	for (const auto& stage : pimpl()->children()) {
+		if (!stage->solutions().empty())
+			continue;  // skip deeper traversal, this stage produced solutions
+		if (stage->numFailures()) {
+			os << stage->name() << " (0/" << stage->numFailures() << ")";
+			stage->explainFailure(os);
+			os << std::endl;
+			break;
+		}
+		stage->explainFailure(os);  // recursively process children
+	}
+}
+
 std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
 	ContainerBase::StageCallback processor = [&os](const Stage& stage, unsigned int depth) -> bool {
 		os << std::string(2 * depth, ' ') << *stage.pimpl() << std::endl;
@@ -690,6 +704,7 @@ void SerialContainer::compute() {
 	}
 }
 
+
 ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
   : ContainerBasePrivate(me, name) {}
 

core/src/stage.cpp

@@ -428,6 +428,11 @@ bool Stage::storeFailures() const {
 	return pimpl()->storeFailures();
 }
 
+void Stage::explainFailure(std::ostream& os) const {
+	if (!failures().empty())
+		os << ": " << failures().front()->comment();
+}
+
 PropertyMap& Stage::properties() {
 	return pimpl()->properties_;
 }

core/src/task.cpp

@@ -237,9 +237,12 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
 	init();
 
 	// Print state and return success if there are solutions otherwise the input error_code
-	const auto success_or = [this](const int32_t error_code) {
+	const auto success_or = [this](const int32_t error_code) -> int32_t {
+		if (numSolutions() > 0)
+			return moveit::core::MoveItErrorCode::SUCCESS;
 		printState();
-		return numSolutions() > 0 ? moveit::core::MoveItErrorCode::SUCCESS : error_code;
+		explainFailure();
+		return error_code;
 	};
 	impl->preempt_requested_ = false;
 	const double available_time = timeout();
@@ -313,5 +316,10 @@ const core::RobotModelConstPtr& Task::getRobotModel() const {
 void Task::printState(std::ostream& os) const {
 	os << *stages();
 }
+
+void Task::explainFailure(std::ostream& os) const {
+	os << "Failing stage(s):\n";
+	stages()->explainFailure(os);
+}
 }  // namespace task_constructor
 }  // namespace moveit