fixup! MoveTo: store goal as any type

This commit is contained in:
Robert Haschke 2018-06-07 13:56:32 +02:00
parent ca9d50e7d8
commit c70db65578

View File

@ -81,7 +81,9 @@ bool MoveTo::getJointStateGoal(const boost::any& goal,
if (!state.setToDefaultValues(jmg, named_joint_pose)) if (!state.setToDefaultValues(jmg, named_joint_pose))
throw InitStageException(*this, "Unknown joint pose: " + named_joint_pose); throw InitStageException(*this, "Unknown joint pose: " + named_joint_pose);
return true; return true;
} catch (const boost::bad_any_cast&) {}
try {
// try RobotState // try RobotState
const moveit_msgs::RobotState& msg = boost::any_cast<moveit_msgs::RobotState>(goal); const moveit_msgs::RobotState& msg = boost::any_cast<moveit_msgs::RobotState>(goal);
if (!msg.is_diff) if (!msg.is_diff)
@ -98,9 +100,7 @@ bool MoveTo::getJointStateGoal(const boost::any& goal,
moveit::core::robotStateMsgToRobotState(msg, state, false); moveit::core::robotStateMsgToRobotState(msg, state, false);
return true; return true;
} catch (const boost::bad_any_cast&) { } catch (const boost::bad_any_cast&) {}
return false;
}
return false; return false;
} }