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 ordered<SolutionBaseConstPtr>& solutions() const;
const std::list<SolutionBaseConstPtr>& failures() const; const std::list<SolutionBaseConstPtr>& failures() const;
size_t numFailures() 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 /// get the stage's property map
PropertyMap& properties(); PropertyMap& properties();

View File

@ -103,7 +103,8 @@ public:
protected: 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); SubTrajectory &trajectory, Direction dir);
protected: protected:

View File

@ -99,7 +99,8 @@ public:
} }
protected: 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); SubTrajectory &trajectory, Direction dir);
bool getJointStateGoal(moveit::core::RobotState& state); bool getJointStateGoal(moveit::core::RobotState& state);

View File

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

View File

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

View File

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