Correctly report errors for other stages too

moveit · Feb 2, 2025 · acb31e2 · acb31e2
1 parent d1cead7
commit acb31e2
Show file tree

Hide file tree

Showing 4 changed files with 30 additions and 4 deletions.
diff --git a/core/src/stages/current_state.cpp b/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

diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/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;
 		}
 

diff --git a/core/src/stages/generate_pose.cpp b/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;
 	}
 

diff --git a/core/src/stages/generate_random_pose.cpp b/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;
 	}