diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp index c185a6772..7c3db3848 100644 --- 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 +#include #include #include @@ -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 index 068f2759c..c66f357c9 100644 --- a/core/src/stages/fixed_cartesian_poses.cpp +++ b/core/src/stages/fixed_cartesian_poses.cpp @@ -34,6 +34,8 @@ /* Authors: Robert Haschke */ +#include + #include #include #include @@ -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 index 5a0f033d4..8a397f30b 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -40,6 +40,7 @@ #include #include #include +#include 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 index 51cf9c533..efb69dab3 100644 --- a/core/src/stages/generate_random_pose.cpp +++ b/core/src/stages/generate_random_pose.cpp @@ -43,6 +43,7 @@ #include #include +#include #include 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; }