MoveTo: reduce scope of try-catch

This commit is contained in:
v4hn 2018-05-30 18:44:26 +02:00
parent bbd274da96
commit b8e6e5e2fa

View File

@ -129,11 +129,19 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
bool success = false;
try {
size_t cartesian_goals = props.countDefined({"pose", "point"});
if (getJointStateGoal(scene->getCurrentStateNonConst())) {
bool has_joint_state_goal;
try {
has_joint_state_goal= getJointStateGoal(scene->getCurrentStateNonConst());
} catch (const InitStageException &e) {
ROS_WARN_STREAM_NAMED("MoveTo", e.what());
return false;
}
if (has_joint_state_goal) {
if (cartesian_goals > 0)
ROS_WARN_NAMED("MoveTo", "Ignoring Cartesian goals in favour of joint space goal");
ROS_WARN_NAMED("MoveTo", "Ignoring Cartesian goals in favor of joint space goal");
// plan to joint-space target
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
} else if (cartesian_goals == 1) {
@ -190,10 +198,6 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
ROS_WARN_NAMED("MoveTo", "Cannot plan to multiple goals");
else if (cartesian_goals == 0)
ROS_WARN_NAMED("MoveTo", "No goal defined");
} catch (const InitStageException &e) {
ROS_WARN_STREAM_NAMED("MoveTo", e.what());
return false;
}
// store result
if (success || (robot_trajectory && storeFailures())) {