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 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();
|
||||||
|
|||||||
@ -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:
|
||||||
|
|||||||
@ -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);
|
||||||
|
|
||||||
|
|||||||
@ -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()
|
||||||
{
|
{
|
||||||
|
|||||||
@ -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();
|
||||||
}
|
}
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -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();
|
||||||
}
|
}
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user