Correctly report errors for other stages too

Author: rhaschke

core/src/stages/current_state.cpp

@@ -96,7 +96,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

@@ -86,7 +86,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_random_pose.cpp

@@ -94,7 +94,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;
 	}