mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fix: don't report config issues as successful (but empty) solutions
This commit is contained in:
parent
381210f43d
commit
2e9932cfbc
@ -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();
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -272,6 +272,10 @@ size_t Stage::numFailures() const
|
||||
return pimpl()->num_failures_;
|
||||
}
|
||||
|
||||
void Stage::silentFailure() {
|
||||
++(pimpl()->num_failures_);
|
||||
}
|
||||
|
||||
|
||||
PropertyMap &Stage::properties()
|
||||
{
|
||||
|
||||
@ -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);
|
||||
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);
|
||||
if (compute(to, from, trajectory, BACKWARD))
|
||||
sendBackward(InterfaceState(from), to, std::move(trajectory));
|
||||
else
|
||||
silentFailure();
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -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);
|
||||
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);
|
||||
if (compute(to, from, trajectory, BACKWARD))
|
||||
sendBackward(InterfaceState(from), to, std::move(trajectory));
|
||||
else
|
||||
silentFailure();
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user