From 2e9932cfbc45356e135a237a3f25b1ee08ad072a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 2 Jun 2018 14:51:36 +0200 Subject: [PATCH] fix: don't report config issues as successful (but empty) solutions --- core/include/moveit/task_constructor/stage.h | 2 ++ .../task_constructor/stages/move_relative.h | 3 +- .../moveit/task_constructor/stages/move_to.h | 3 +- core/src/stage.cpp | 4 +++ core/src/stages/move_relative.cpp | 25 +++++++++------- core/src/stages/move_to.cpp | 29 +++++++++++-------- 6 files changed, 42 insertions(+), 24 deletions(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 58efe2a2..ef56e267 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -169,6 +169,8 @@ public: const ordered& solutions() const; const std::list& 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(); diff --git a/core/include/moveit/task_constructor/stages/move_relative.h b/core/include/moveit/task_constructor/stages/move_relative.h index fd60928c..6b35ae91 100644 --- a/core/include/moveit/task_constructor/stages/move_relative.h +++ b/core/include/moveit/task_constructor/stages/move_relative.h @@ -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: diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index 7c67a23c..65e88a7b 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -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); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 1eb6cc10..58e08960 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -272,6 +272,10 @@ size_t Stage::numFailures() const return pimpl()->num_failures_; } +void Stage::silentFailure() { + ++(pimpl()->num_failures_); +} + PropertyMap &Stage::properties() { diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 871ebb13..f8ed2146 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -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("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(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(); } } } } diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 96183115..cfb8ea3f 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -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("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(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(); } } } }