ComputeIK: report failures the modern way

core/include/moveit/task_constructor/container.h

@@ -80,7 +80,7 @@ class ContainerBase : public Stage
 
 	virtual bool canCompute() const = 0;
 	virtual void compute() = 0;
-	void explainFailure(std::ostream& os) const override;
+	bool 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

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

core/include/moveit/task_constructor/task.h

@@ -137,7 +137,7 @@ class Task : protected WrapperBase
 	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;
+	bool explainFailure(std::ostream& os = std::cout) const override;
 
 	size_t numSolutions() const { return solutions().size(); }
 	const ordered<SolutionBaseConstPtr>& solutions() const { return stages()->solutions(); }

core/src/container.cpp

@@ -452,18 +452,21 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
 		throw errors;
 }
 
-void ContainerBase::explainFailure(std::ostream& os) const {
+bool 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);
+			if (!stage->failures().empty())
+				os << ": " << stage->failures().front()->comment();
 			os << '\n';
-			break;
+			return true;
 		}
-		stage->explainFailure(os);  // recursively process children
+		if (stage->explainFailure(os))  // recursively process children
+			return true;
 	}
+	return false;
 }
 
 std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {

core/src/stage.cpp

@@ -436,11 +436,6 @@ 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/stages/compute_ik.cpp

@@ -251,16 +251,23 @@ void ComputeIK::compute() {
 	const moveit::core::JointModelGroup* jmg = nullptr;
 	std::string msg;
 
+	auto report_failure = [&s, this](const std::string& msg) {
+		planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
+		SubTrajectory solution;
+		solution.markAsFailure(msg);
+		spawn(InterfaceState(scene), std::move(solution));
+	};
+
 	if (!validateEEF(props, robot_model, eef_jmg, &msg)) {
-		ROS_WARN_STREAM_NAMED("ComputeIK", msg);
+		report_failure(msg);
 		return;
 	}
 	if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg)) {
-		ROS_WARN_STREAM_NAMED("ComputeIK", msg);
+		report_failure(msg);
 		return;
 	}
 	if (!eef_jmg && !jmg) {
-		ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined");
+		report_failure(msg);
 		return;
 	}
 	properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout());
@@ -274,8 +281,7 @@ void ComputeIK::compute() {
 	tf2::fromMsg(target_pose_msg.pose, target_pose);
 	if (target_pose_msg.header.frame_id != scene->getPlanningFrame()) {
 		if (!scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
-			ROS_WARN_STREAM_NAMED("ComputeIK",
-			                      "Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
+			report_failure(fmt::format("Unknown reference frame for target pose: '{}'", target_pose_msg.header.frame_id));
 			return;
 		}
 		// transform target_pose w.r.t. planning frame
@@ -290,7 +296,7 @@ void ComputeIK::compute() {
 		//  determine IK link from eef/group
 		if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) :
 		                       jmg->getOnlyOneEndEffectorTip())) {
-			ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link");
+			report_failure("Failed to derive IK target link");
 			return;
 		}
 		ik_pose_msg.header.frame_id = link->getName();
@@ -301,8 +307,7 @@ void ComputeIK::compute() {
 		tf2::fromMsg(ik_pose_msg.pose, ik_pose);
 
 		if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) {
-			ROS_WARN_STREAM_NAMED("ComputeIK",
-			                      fmt::format("ik frame unknown in robot: '{}'", ik_pose_msg.header.frame_id));
+			report_failure(fmt::format("ik frame unknown in robot: '{}'", ik_pose_msg.header.frame_id));
 			return;
 		}
 		ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;

core/src/stages/current_state.cpp

@@ -36,6 +36,7 @@
 /* Authors: Michael Goerner, Luca Lach, Robert Haschke */
 
 #include <chrono>
+#include <fmt/format.h>
 
 #include <moveit/task_constructor/stages/current_state.h>
 #include <moveit/task_constructor/storage.h>
@@ -96,7 +97,13 @@ void CurrentState::compute() {
 			return;
 		}
 	}
-	ROS_WARN("failed to acquire current PlanningScene");
+	if (storeFailures()) {
+		SubTrajectory solution;
+		solution.markAsFailure(
+		    fmt::format("Failed to acquire current PlanningScene within timeout of {}s", timeout.toSec()));
+		spawn(InterfaceState(scene_), std::move(solution));
+	} else
+		ROS_WARN_STREAM_NAMED("CurrentState", "Failed to acquire current PlanningScene");
 }
 }  // namespace stages
 }  // namespace task_constructor

core/src/stages/fixed_cartesian_poses.cpp

@@ -34,6 +34,8 @@
 
 /* Authors: Robert Haschke */
 
+#include <fmt/format.h>
+
 #include <moveit/task_constructor/stages/fixed_cartesian_poses.h>
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/cost_terms.h>
@@ -86,7 +88,12 @@ void FixedCartesianPoses::compute() {
 		if (pose.header.frame_id.empty())
 			pose.header.frame_id = scene->getPlanningFrame();
 		else if (!scene->knowsFrameTransform(pose.header.frame_id)) {
-			ROS_WARN_NAMED("FixedCartesianPoses", "Unknown frame: '%s'", pose.header.frame_id.c_str());
+			if (storeFailures()) {
+				SubTrajectory trajectory;
+				trajectory.markAsFailure(fmt::format("Unknown frame: '{}'", pose.header.frame_id));
+				spawn(InterfaceState(scene), std::move(trajectory));
+			} else
+				ROS_WARN_NAMED("FixedCartesianPoses", "Unknown frame: '%s'", pose.header.frame_id.c_str());
 			continue;
 		}
 

core/src/stages/generate_pose.cpp

@@ -40,6 +40,7 @@
 #include <moveit/task_constructor/marker_tools.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <rviz_marker_tools/marker_creation.h>
+#include <fmt/format.h>
 
 namespace moveit {
 namespace task_constructor {
@@ -77,7 +78,12 @@ void GeneratePose::compute() {
 	if (target_pose.header.frame_id.empty())
 		target_pose.header.frame_id = scene->getPlanningFrame();
 	else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) {
-		ROS_WARN_NAMED("GeneratePose", "Unknown frame: '%s'", target_pose.header.frame_id.c_str());
+		if (storeFailures()) {
+			SubTrajectory trajectory;
+			trajectory.markAsFailure(fmt::format("Unknown frame: '{}'", target_pose.header.frame_id));
+			spawn(InterfaceState(scene), std::move(trajectory));
+		} else
+			ROS_WARN_NAMED("GeneratePose", "Unknown frame: '%s'", target_pose.header.frame_id.c_str());
 		return;
 	}
 

core/src/stages/generate_random_pose.cpp

@@ -43,6 +43,7 @@
 #include <Eigen/Geometry>
 #include <tf2_eigen/tf2_eigen.h>
 
+#include <fmt/format.h>
 #include <chrono>
 
 namespace {
@@ -94,7 +95,12 @@ void GenerateRandomPose::compute() {
 	if (seed_pose.header.frame_id.empty())
 		seed_pose.header.frame_id = scene->getPlanningFrame();
 	else if (!scene->knowsFrameTransform(seed_pose.header.frame_id)) {
-		ROS_WARN_NAMED("GenerateRandomPose", "Unknown frame: '%s'", seed_pose.header.frame_id.c_str());
+		if (storeFailures()) {
+			SubTrajectory trajectory;
+			trajectory.markAsFailure(fmt::format("Unknown frame: '{}'", seed_pose.header.frame_id));
+			spawn(InterfaceState(scene), std::move(trajectory));
+		} else
+			ROS_WARN_NAMED("GenerateRandomPose", "Unknown frame: '%s'", seed_pose.header.frame_id.c_str());
 		return;
 	}
 

core/src/task.cpp

@@ -333,9 +333,9 @@ void Task::printState(std::ostream& os) const {
 	os << *stages();
 }
 
-void Task::explainFailure(std::ostream& os) const {
+bool Task::explainFailure(std::ostream& os) const {
 	os << "Failing stage(s):\n";
-	stages()->explainFailure(os);
+	return stages()->explainFailure(os);
 }
 }  // namespace task_constructor
 }  // namespace moveit