mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Correctly report failures instead of issueing console warnings
Don't use command-line warnings, but spawn failure solutions.
This commit is contained in:
parent
8318546992
commit
9ea1692f96
@ -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;
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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(); }
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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_;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user