MoveRel: handle unknown links with hard failure

This commit is contained in:
v4hn 2018-05-30 19:09:57 +02:00
parent 41e64ed7e1
commit 015b1466f4

View File

@ -103,12 +103,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
const auto& accepted = jmg->getJointModelNames();
auto& robot_state = scene->getCurrentStateNonConst();
const auto& joints = boost::any_cast<std::map<std::string, double>>(goal);
for (auto j : joints) {
for (const auto& j : joints) {
int index = robot_state.getRobotModel()->getVariableIndex(j.first);
auto jm = scene->getRobotModel()->getJointModel(index);
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end()) {
ROS_WARN_STREAM_NAMED("MoveRelative", "Ignoring joint " << j.first << " that is not part of group " << group);
continue;
ROS_WARN_STREAM_NAMED("MoveRelative", "Cannot plan joint target for joint '" << j.first << "' that is not part of group '" << group << "'");
return false;
}
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
robot_state.enforceBounds(jm);