fix: don't report config issues as successful (but empty) solutions

This commit is contained in:
Robert Haschke 2018-06-02 14:51:36 +02:00
parent 381210f43d
commit 2e9932cfbc
6 changed files with 42 additions and 24 deletions

View File

@ -169,6 +169,8 @@ public:
const ordered<SolutionBaseConstPtr>& solutions() const;
const std::list<SolutionBaseConstPtr>& failures() const;
size_t numFailures() const;
/// call to increase number of failures w/o storing a (failure) trajectory
void silentFailure();
/// get the stage's property map
PropertyMap& properties();

View File

@ -103,7 +103,8 @@ public:
protected:
void compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
// return false if trajectory shouldn't be stored
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
SubTrajectory &trajectory, Direction dir);
protected:

View File

@ -99,7 +99,8 @@ public:
}
protected:
void compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
// return false if trajectory shouldn't be stored
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
SubTrajectory &trajectory, Direction dir);
bool getJointStateGoal(moveit::core::RobotState& state);

View File

@ -272,6 +272,10 @@ size_t Stage::numFailures() const
return pimpl()->num_failures_;
}
void Stage::silentFailure() {
++(pimpl()->num_failures_);
}
PropertyMap &Stage::properties()
{

View File

@ -76,7 +76,7 @@ void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
planner_->init(robot_model);
}
void MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
SubTrajectory &solution, Direction dir) {
scene = state.scene()->diff();
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
@ -88,7 +88,7 @@ void MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
if (!jmg) {
ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group);
return;
return false;
}
// only allow single target
@ -96,7 +96,7 @@ void MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
if (count_goals != 1) {
if (count_goals == 0) ROS_WARN_NAMED("MoveRelative", "No goal defined");
else ROS_WARN_NAMED("MoveRelative", "Cannot plan to multiple goals");
return;
return false;
}
double max_distance = props.get<double>("max_distance");
@ -116,7 +116,7 @@ void MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
auto jm = robot_model->getJointModel(index);
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end()) {
ROS_WARN_STREAM_NAMED("MoveRelative", "Cannot plan joint target for joint '" << j.first << "' that is not part of group '" << group << "'");
return;
return false;
}
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
robot_state.enforceBounds(jm);
@ -132,7 +132,7 @@ void MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
// determine IK link from group
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("MoveRelative", "Failed to derive IK target link");
return;
return false;
}
ik_pose_msg.header.frame_id = link->getName();
ik_pose_msg.pose.orientation.w = 1.0;
@ -140,7 +140,7 @@ void MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
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);
return;
return false;
}
}
@ -276,6 +276,7 @@ void MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
if (!success)
solution.markAsFailure();
}
return true;
}
void MoveRelative::computeForward(const InterfaceState &from)
@ -283,8 +284,10 @@ void MoveRelative::computeForward(const InterfaceState &from)
planning_scene::PlanningScenePtr to;
SubTrajectory trajectory;
compute(from, to, trajectory, FORWARD);
sendForward(from, InterfaceState(to), std::move(trajectory));
if (compute(from, to, trajectory, FORWARD))
sendForward(from, InterfaceState(to), std::move(trajectory));
else
silentFailure();
}
void MoveRelative::computeBackward(const InterfaceState &to)
@ -292,8 +295,10 @@ void MoveRelative::computeBackward(const InterfaceState &to)
planning_scene::PlanningScenePtr from;
SubTrajectory trajectory;
compute(to, from, trajectory, BACKWARD);
sendBackward(InterfaceState(from), to, std::move(trajectory));
if (compute(to, from, trajectory, BACKWARD))
sendBackward(InterfaceState(from), to, std::move(trajectory));
else
silentFailure();
}
} } }

View File

@ -119,7 +119,7 @@ bool MoveTo::getJointStateGoal(moveit::core::RobotState& state) {
return false;
}
void MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
SubTrajectory &solution, Direction dir) {
scene = state.scene()->diff();
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
@ -131,7 +131,7 @@ void MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
if (!jmg) {
ROS_WARN_STREAM_NAMED("MoveTo", "Invalid joint model group: " << group);
return;
return false;
}
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
@ -143,24 +143,24 @@ void MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
has_joint_state_goal = getJointStateGoal(scene->getCurrentStateNonConst());
} catch (const InitStageException &e) {
ROS_WARN_STREAM_NAMED("MoveTo", e.what());
return;
return false;
}
size_t cartesian_goals = props.countDefined({"pose", "point"});
if (cartesian_goals > 1) {
ROS_WARN_NAMED("MoveTo", "Ambiguous goals: Multiple Cartesian goals defined");
return;
return false;
}
if (cartesian_goals > 0 && has_joint_state_goal) {
ROS_WARN_NAMED("MoveTo", "Ambiguous goals: Cartesian goals and joint state goals defined");
return;
return false;
}
if (cartesian_goals == 0 && !has_joint_state_goal) {
ROS_WARN_NAMED("MoveTo", "No goal defined");
return;
return false;
}
if (has_joint_state_goal) {
@ -177,7 +177,7 @@ void MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
// determine IK link from group
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("MoveTo", "Failed to derive IK target link");
return;
return false;
}
ik_pose_msg.header.frame_id = link->getName();
ik_pose_msg.pose.orientation.w = 1.0;
@ -185,7 +185,7 @@ void MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
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);
return;
return false;
}
}
@ -237,6 +237,7 @@ void MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
if (!success)
solution.markAsFailure();
}
return true;
}
// -1 TODO: move these as default implementation to PropagateEitherWay?
@ -245,8 +246,10 @@ void MoveTo::computeForward(const InterfaceState &from){
planning_scene::PlanningScenePtr to;
SubTrajectory trajectory;
compute(from, to, trajectory, FORWARD);
sendForward(from, InterfaceState(to), std::move(trajectory));
if (compute(from, to, trajectory, FORWARD))
sendForward(from, InterfaceState(to), std::move(trajectory));
else
silentFailure();
}
void MoveTo::computeBackward(const InterfaceState &to)
@ -254,8 +257,10 @@ void MoveTo::computeBackward(const InterfaceState &to)
planning_scene::PlanningScenePtr from;
SubTrajectory trajectory;
compute(to, from, trajectory, BACKWARD);
sendBackward(InterfaceState(from), to, std::move(trajectory));
if (compute(to, from, trajectory, BACKWARD))
sendBackward(InterfaceState(from), to, std::move(trajectory));
else
silentFailure();
}
} } }