Correctly report failures instead of issueing console warnings

Don't use command-line warnings, but spawn failure solutions.
This commit is contained in:
Robert Haschke 2025-02-06 14:09:15 +00:00 committed by GitHub
parent 8318546992
commit 9ea1692f96
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
11 changed files with 55 additions and 26 deletions

View File

@ -80,7 +80,7 @@ public:
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;

View File

@ -242,7 +242,7 @@ public:
/// 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();

View File

@ -137,7 +137,7 @@ public:
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(); }

View File

@ -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) {

View File

@ -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_;
}

View File

@ -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;

View File

@ -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

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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