diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 24e3b1ec4..cfb4a1e52 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/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; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index f75a1be59..82d01c765 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/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(); diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index d1c123740..0b07d0601 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/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& solutions() const { return stages()->solutions(); } diff --git a/core/src/container.cpp b/core/src/container.cpp index 3fe6f1b5f..c1fc23f11 100644 --- a/core/src/container.cpp +++ b/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) { diff --git a/core/src/stage.cpp b/core/src/stage.cpp index ec72c3d66..789da4651 100644 --- a/core/src/stage.cpp +++ b/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_; } diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 9ae900043..8d7265c2f 100644 --- a/core/src/stages/compute_ik.cpp +++ b/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; 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; } diff --git a/core/src/task.cpp b/core/src/task.cpp index ab375d2c7..221bb6aca 100644 --- a/core/src/task.cpp +++ b/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