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

View File

@ -149,6 +149,12 @@ void SolutionBase::setCost(double 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 {
info.id = introspection ? introspection->solutionId(*this) : 0;
info.cost = this->cost();