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

Author: rhaschke

core/include/moveit/task_constructor/storage.h

@@ -235,7 +235,7 @@ class SolutionBase
 
 	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_; }

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<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,15 +142,15 @@ 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();
 			ik_pose_msg.pose.orientation.w = 1.0;
 		} 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

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<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,22 +223,22 @@ 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();
 			ik_pose_msg.pose.orientation.w = 1.0;
 		} 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

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<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();