From dfffd77eee45c1806a0a12fe686446696186f4c8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 18 Mar 2021 17:20:17 +0100 Subject: [PATCH] MoveTo/MoveRel: Report errors via solution comments instead of ROS_WARN --- .../include/moveit/task_constructor/storage.h | 2 +- core/src/stages/move_relative.cpp | 22 +++++++++---------- core/src/stages/move_to.cpp | 22 +++++++++---------- core/src/storage.cpp | 6 +++++ 4 files changed, 29 insertions(+), 23 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 6896e2a6..5832e250 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -235,7 +235,7 @@ public: inline double cost() const { return cost_; } void setCost(double cost); - void markAsFailure() { setCost(std::numeric_limits::infinity()); } + void markAsFailure(const std::string& msg = std::string()); inline bool isFailure() const { return !std::isfinite(cost_); } const std::string& comment() const { return comment_; } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index e5266825..9ba9b262 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -115,12 +115,12 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning const std::string& group = props.get("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(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 diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index ab7f052c..d585008d 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -197,12 +197,12 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP const std::string& group = props.get("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(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 diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 93a49ded..ad5e8314 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -149,6 +149,12 @@ void SolutionBase::setCost(double cost) { cost_ = cost; } +void SolutionBase::markAsFailure(const std::string& msg) { + setCost(std::numeric_limits::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();