MoveTo/MoveRel: Report errors via solution comments instead of ROS_WARN

This commit is contained in:
Robert Haschke 2021-03-18 17:20:17 +01:00
parent 5de9ce7203
commit dfffd77eee
4 changed files with 29 additions and 23 deletions

View File

@ -235,7 +235,7 @@ public:
inline double cost() const { return cost_; } inline double cost() const { return cost_; }
void setCost(double cost); void setCost(double cost);
void markAsFailure() { setCost(std::numeric_limits<double>::infinity()); } void markAsFailure(const std::string& msg = std::string());
inline bool isFailure() const { return !std::isfinite(cost_); } inline bool isFailure() const { return !std::isfinite(cost_); }
const std::string& comment() const { return comment_; } const std::string& comment() const { return comment_; }

View File

@ -115,12 +115,12 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
const std::string& group = props.get<std::string>("group"); const std::string& group = props.get<std::string>("group");
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
if (!jmg) { if (!jmg) {
ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group); solution.markAsFailure("invalid joint model group: " + group);
return false; return false;
} }
boost::any direction = props.get("direction"); boost::any direction = props.get("direction");
if (direction.empty()) { if (direction.empty()) {
ROS_WARN_NAMED("MoveRelative", "direction undefined"); solution.markAsFailure("undefined direction");
return false; return false;
} }
@ -142,7 +142,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
if (value.empty()) { // property undefined if (value.empty()) { // property undefined
// determine IK link from group // determine IK link from group
if (!(link = jmg->getOnlyOneEndEffectorTip())) { if (!(link = jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("MoveRelative", "Failed to derive IK target link"); solution.markAsFailure("missing ik_frame");
return false; return false;
} }
ik_pose_msg.header.frame_id = link->getName(); ik_pose_msg.header.frame_id = link->getName();
@ -150,7 +150,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
} else { } else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value); ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) { if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
ROS_WARN_STREAM_NAMED("MoveRelative", "Unknown link: " << ik_pose_msg.header.frame_id); solution.markAsFailure("unknown link for ik_frame: " + ik_pose_msg.header.frame_id);
return false; return false;
} }
} }
@ -228,7 +228,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
target_eigen = link_pose; target_eigen = link_pose;
target_eigen.translation() += linear; target_eigen.translation() += linear;
} catch (const boost::bad_any_cast&) { } catch (const boost::bad_any_cast&) {
ROS_ERROR_STREAM_NAMED("MoveRelative", "Invalid type for direction: " << direction.type().name()); solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name());
return false; return false;
} }
@ -306,20 +306,20 @@ void MoveRelative::computeForward(const InterfaceState& from) {
planning_scene::PlanningScenePtr to; planning_scene::PlanningScenePtr to;
SubTrajectory trajectory; SubTrajectory trajectory;
if (compute(from, to, trajectory, FORWARD)) if (!compute(from, to, trajectory, FORWARD) && trajectory.comment().empty())
sendForward(from, InterfaceState(to), std::move(trajectory)); silentFailure(); // there is nothing to report (comment is empty)
else else
silentFailure(); sendForward(from, InterfaceState(to), std::move(trajectory));
} }
void MoveRelative::computeBackward(const InterfaceState& to) { void MoveRelative::computeBackward(const InterfaceState& to) {
planning_scene::PlanningScenePtr from; planning_scene::PlanningScenePtr from;
SubTrajectory trajectory; SubTrajectory trajectory;
if (compute(to, from, trajectory, BACKWARD)) if (!compute(to, from, trajectory, BACKWARD) && trajectory.comment().empty())
sendBackward(InterfaceState(from), to, std::move(trajectory));
else
silentFailure(); silentFailure();
else
sendBackward(InterfaceState(from), to, std::move(trajectory));
} }
} // namespace stages } // namespace stages
} // namespace task_constructor } // namespace task_constructor

View File

@ -197,12 +197,12 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
const std::string& group = props.get<std::string>("group"); const std::string& group = props.get<std::string>("group");
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
if (!jmg) { if (!jmg) {
ROS_WARN_STREAM_NAMED("MoveTo", "Invalid joint model group: " << group); solution.markAsFailure("invalid joint model group: " + group);
return false; return false;
} }
boost::any goal = props.get("goal"); boost::any goal = props.get("goal");
if (goal.empty()) { if (goal.empty()) {
ROS_WARN_NAMED("MoveTo", "goal undefined"); solution.markAsFailure("undefined goal");
return false; return false;
} }
@ -223,7 +223,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
if (value.empty()) { // property undefined if (value.empty()) { // property undefined
// determine IK link from group // determine IK link from group
if (!(link = jmg->getOnlyOneEndEffectorTip())) { if (!(link = jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("MoveTo", "Failed to derive IK target link"); solution.markAsFailure("missing ik_frame");
return false; return false;
} }
ik_pose_msg.header.frame_id = link->getName(); ik_pose_msg.header.frame_id = link->getName();
@ -231,14 +231,14 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
} else { } else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value); ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) { if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
ROS_WARN_STREAM_NAMED("MoveTo", "Unknown link: " << ik_pose_msg.header.frame_id); solution.markAsFailure("unknown link for ik_frame: " + ik_pose_msg.header.frame_id);
return false; return false;
} }
} }
if (!getPoseGoal(goal, ik_pose_msg, scene, target_eigen, solution.markers()) && if (!getPoseGoal(goal, ik_pose_msg, scene, target_eigen, solution.markers()) &&
!getPointGoal(goal, link, scene, target_eigen, solution.markers())) { !getPointGoal(goal, link, scene, target_eigen, solution.markers())) {
ROS_ERROR_STREAM_NAMED("MoveTo", "Invalid type for goal: " << goal.type().name()); solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name());
return false; return false;
} }
@ -275,20 +275,20 @@ void MoveTo::computeForward(const InterfaceState& from) {
planning_scene::PlanningScenePtr to; planning_scene::PlanningScenePtr to;
SubTrajectory trajectory; SubTrajectory trajectory;
if (compute(from, to, trajectory, FORWARD)) if (!compute(from, to, trajectory, FORWARD) && trajectory.comment().empty())
sendForward(from, InterfaceState(to), std::move(trajectory)); silentFailure(); // there is nothing to report (comment is empty)
else else
silentFailure(); sendForward(from, InterfaceState(to), std::move(trajectory));
} }
void MoveTo::computeBackward(const InterfaceState& to) { void MoveTo::computeBackward(const InterfaceState& to) {
planning_scene::PlanningScenePtr from; planning_scene::PlanningScenePtr from;
SubTrajectory trajectory; SubTrajectory trajectory;
if (compute(to, from, trajectory, BACKWARD)) if (!compute(to, from, trajectory, BACKWARD) && trajectory.comment().empty())
sendBackward(InterfaceState(from), to, std::move(trajectory));
else
silentFailure(); silentFailure();
else
sendBackward(InterfaceState(from), to, std::move(trajectory));
} }
} // namespace stages } // namespace stages
} // namespace task_constructor } // namespace task_constructor

View File

@ -149,6 +149,12 @@ void SolutionBase::setCost(double cost) {
cost_ = cost; cost_ = cost;
} }
void SolutionBase::markAsFailure(const std::string& msg) {
setCost(std::numeric_limits<double>::infinity());
if (!msg.empty())
setComment(msg + "\n" + comment());
}
void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection) const { void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection) const {
info.id = introspection ? introspection->solutionId(*this) : 0; info.id = introspection ? introspection->solutionId(*this) : 0;
info.cost = this->cost(); info.cost = this->cost();